Skip to content

Commit

Permalink
Merge pull request #1143 from paparazzi/opticflow
Browse files Browse the repository at this point in the history 
[vision] Opticflow rewrite: 
-less grabbing threads
-faster grabbing
-static linking issues fixed properly
-documentation
-rewritten code
  • Loading branch information
@dewagter
dewagter committed Apr 9, 2015
2 parents 4f5806a + dea8152 commit ea58643
Show file tree
Hide file tree
Showing 49 changed files with 5,572 additions and 9,626 deletions.
6 changes: 3 additions & 3 deletions conf/Makefile.arm-linux
View file
Expand Up @@ -39,12 +39,12 @@ else
FLOAT_ABI ?= -mfloat-abi=softfp -mfpu=vfp
endif

ARCH_FLAGS ?= -mtune=cortex-a8 -march=armv7-a
ARCH_CFLAGS ?= -mtune=cortex-a8 -march=armv7-a

# add ARM specifc flags to CFLAGS, LDFLAGS
CFLAGS += $(FLOAT_ABI) $(ARCH_FLAGS)
CFLAGS += $(FLOAT_ABI) $(ARCH_CFLAGS)
LDFLAGS += $(FLOAT_ABI)
CXXFLAGS += $(FLOAT_ABI) $(ARCH_FLAGS)
CXXFLAGS += $(FLOAT_ABI) $(ARCH_CFLAGS)

# include the common linux Makefile (common CFLAGS, actual targets)
include $(PAPARAZZI_SRC)/conf/Makefile.linux
4 changes: 4 additions & 0 deletions conf/boards/ardrone2_raw.makefile
View file
Expand Up @@ -42,6 +42,10 @@ $(TARGET).CFLAGS +=-DARDRONE2_RAW
# handle linux signals by hand
$(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL

# Link static (Done for GLIBC)
$(TARGET).CFLAGS += -DLINUX_LINK_STATIC
$(TARGET).LDFLAGS += -static

# -----------------------------------------------------------------------

# default LED configuration
Expand Down
4 changes: 4 additions & 0 deletions conf/boards/bebop.makefile
View file
Expand Up @@ -34,6 +34,10 @@ $(TARGET).CFLAGS += -DUSE_LINUX_SIGNAL
# Compile the video specific parts
$(TARGET).srcs += $(SRC_BOARD)/video.c

# Link static (Done for GLIBC)
$(TARGET).CFLAGS += -DLINUX_LINK_STATIC
$(TARGET).LDFLAGS += -static

# -----------------------------------------------------------------------

# default LED configuration
Expand Down
39 changes: 15 additions & 24 deletions conf/messages.xml
View file
Expand Up @@ -1972,30 +1972,21 @@
<field name="t" type="uint32"/>
</message>

<message name="OF_HOVER" id="228">
<field name="FPS" type="float"/>
<field name="dx" type="float"/>
<field name="dy" type="float"/>
<field name="dx_trans" type="float"/>
<field name="dy_trans" type="float"/>
<field name="diff_roll" type="float"/>
<field name="diff_pitch" type="float"/>
<field name="velx" type="float"/>
<field name="vely" type="float"/>
<field name="velx_Ned" type="float"/>
<field name="vely_Ned" type="float"/>
<field name="z_sonar" type="float"/>
<field name="count" type="int32"/>
</message>

<message name="VISION_STABILIZATION" id="229">
<field name="velx" type="float"/>
<field name="vely" type="float"/>
<field name="velx_i" type="float"/>
<field name="vely_i" type="float"/>
<field name="cmd_phi" type="int32"/>
<field name="cmd_theta" type="int32"/>
</message>
<message name="OPTIC_FLOW_EST" id="228">
<field name="fps" type="float"/>
<field name="corner_cnt" type="uint16"/>
<field name="tracked_cnt" type="uint16"/>
<field name="flow_x" type="int16" unit="subpixels"/>
<field name="flow_y" type="int16" unit="subpixels"/>
<field name="flow_der_x" type="int16" unit="subpixels"/>
<field name="flow_der_y" type="int16" unit="subpixels"/>
<field name="vel_x" type="float" unit="cm/s"/>
<field name="vel_y" type="float" unit="cm/s"/>
<field name="cmd_phi" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
<field name="cmd_theta" type="int32" alt_unit="deg" alt_unit_coef="0.0139882"/>
</message>

<!--229 is free -->

<message name="AHRS_ARDRONE2" id="230">
<field name="state" type="uint32" />
Expand Down
143 changes: 100 additions & 43 deletions conf/modules/cv_opticflow.xml
View file
Expand Up @@ -3,70 +3,127 @@
<module name="cv_opticflow" dir="computer_vision">
<doc>
<description>
Compute Optic Flow from Ardrone2 Bottom Camera

Computes Pitch- and rollrate corrected optic flow from downward looking
ARDrone2 camera looking at a textured floor.
Hovers the drone based on optical flow made for Linux video Devices.

Computes Pitch- and roll attide from downward looking camera looking at a textured floor.
- Sonar is required.
- Controller can hold position
</description>

<!-- Satbilization parameters and gains -->
<section name="VISION" prefix="VISION_">
<define name="HOVER" value="FALSE" description="TRUE/FALSE active or not"/>
<define name="PHI_PGAIN" value="500" description="optic flow pgain"/>
<define name="PHI_IGAIN" value="10" description="optic flow igain"/>
<define name="THETA_PGAIN" value="500" description="optic flow pgain"/>
<define name="THETA_IGAIN" value="10" description="optic flow igain"/>
<define name="DESIRED_VX" value="0" description="feedforward optic flow vx"/>
<define name="DESIRED_VY" value="0" description="feedforward optic flow vy"/>
<define name="PHI_PGAIN" value="400" description="Optic flow proportional gain on the roll velocity error"/>
<define name="PHI_IGAIN" value="20" description="Optic flow integrated gain on the summed roll velocity error"/>
<define name="THETA_PGAIN" value="400" description="Optic flow proportional gain on the pitch velocity error"/>
<define name="THETA_IGAIN" value="20" description="Optic flow integrated gain on the summed pitch velocity error"/>
<define name="DESIRED_VX" value="0" description="The desired velocity in the body frame x direction"/>
<define name="DESIRED_VY" value="0" description="The desired velocity in the body frame y direction"/>
</section>

<!-- Optical flow calculation parameters -->
<section name="OPTICFLOW" prefix="OPTICFLOW_">
<define name="AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>

<!-- Video device parameters -->
<define name="DEVICE" value="/dev/video2" description="The V4L2 camera device that is used for the calculations"/>
<define name="DEVICE_SIZE" value="320,240" description="The V4L2 camera device width and height"/>
<define name="DEVICE_BUFFERS" value="15" description="Amount of V4L2 video buffers"/>
<define name="SUBDEV" description="The V4L2 subdevice to initialize before the main device"/>

<!-- Camera parameters -->
<define name="FOV_W" value="0.89360857702" description="The field of view width of the bottom camera (Defaults are from an ARDrone 2)"/>
<define name="FOV_H" value="0.67020643276" description="The field of view height of the bottom camera (Defaults are from an ARDrone 2)"/>
<define name="FX" value="343.1211" description="Field in the x direction of the camera (Defaults are from an ARDrone 2)"/>
<define name="FY" value="348.5053" description="Field in the y direction of the camera (Defaults are from an ARDrone 2)"/>

<!-- Lucas Kanade optical flow calculation parameters -->
<define name="MAX_TRACK_CORNERS" value="25" description="The maximum amount of corners the Lucas Kanade algorithm is tracking between two frames"/>
<define name="WINDOW_SIZE" value="10" description="Window size used in Lucas Kanade algorithm"/>
<define name="SUBPIXEL_FACTOR" value="10" description="Amount of subpixels per pixel, used for more precise (subpixel) calculations of the flow"/>
<define name="MAX_ITERATIONS" value="10" description="Maximum number of iterations the Lucas Kanade algorithm should take"/>
<define name="THRESHOLD_VEC" value="2" description="TThreshold in subpixels when the iterations of Lucas Kanade should stop"/>

<!-- FAST9 corner detection parameters -->
<define name="FAST9_ADAPTIVE" value="TRUE" description="Whether we should use and adapative FAST9 crner detection threshold"/>
<define name="FAST9_THRESHOLD" value="20" description="FAST9 default threshold"/>
<define name="FAST9_MIN_DISTANCE" value="10" description="The amount of pixels between corners that should be detected"/>
</section>
<define name="DOWNLINK_VIDEO" value="FALSE" description="Also stream video: warning: this makes the optic flow slow: DEBUGGING only" />
<define name="OPTICFLOW_AGL_ID" value="ABI_SENDER_ID" description="ABI sender id for AGL message (sonar measurement) (default: ABI_BROADCAST)"/>
</doc>

<settings>
<dl_settings>
<dl_settings NAME="Vision Loop">
<dl_setting var="activate_opticflow_hover" min="0" step="1" max="1" module="computer_vision/opticflow/hover_stabilization" shortname="hover" param="VISION_HOVER" values="FALSE|TRUE"/>
<dl_setting var="vision_phi_pgain" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
<dl_setting var="vision_phi_igain" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
<dl_setting var="vision_theta_pgain" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
<dl_setting var="vision_theta_igain" min="0" step="1" max="1000" shortname="ki_v_theta" param="VISION_THETA_IGAIN"/>
<dl_setting var="vision_desired_vx" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
<dl_setting var="vision_desired_vy" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
</dl_settings> </dl_settings>
<dl_settings NAME="Vision stabilization">
<!-- Satabilization loop parameters and gains -->
<dl_settings name="vision_stab">
<dl_setting var="opticflow_stab.phi_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_phi" param="VISION_PHI_PGAIN"/>
<dl_setting var="opticflow_stab.phi_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_phi" param="VISION_PHI_IGAIN"/>
<dl_setting var="opticflow_stab.theta_pgain" module="computer_vision/opticflow_module" min="0" step="1" max="10000" shortname="kp_v_theta" param="VISION_THETA_PGAIN"/>
<dl_setting var="opticflow_stab.theta_igain" module="computer_vision/opticflow_module" min="0" step="1" max="1000" shortname="ki_v_theta" param="VISION_THETA_IGAIN"/>
<dl_setting var="opticflow_stab.desired_vx" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vx" param="VISION_DESIRED_VX"/>
<dl_setting var="opticflow_stab.desired_vy" module="computer_vision/opticflow_module" min="-5" step="0.01" max="5" shortname="desired_vy" param="VISION_DESIRED_VY"/>
</dl_settings>

<!-- Optical flow calculations parameters for Lucas Kanade and FAST9 -->
<dl_settings name="vision_calc">
<dl_setting var="opticflow.max_track_corners" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="max_trck_corners" param="OPTICFLOW_MAX_TRACK_CORNERS"/>
<dl_setting var="opticflow.window_size" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="window_size" param="OPTICFLOW_WINDOW_SIZE"/>
<dl_setting var="opticflow.subpixel_factor" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="subpixel_factor" param="OPTICFLOW_SUBPIXEL_FACTOR"/>
<dl_setting var="opticflow.max_iterations" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="max_iterations" param="OPTICFLOW_MAX_ITERATIONS"/>
<dl_setting var="opticflow.threshold_vec" module="computer_vision/opticflow_module" min="0" step="1" max="100" shortname="threshold_vec" param="OPTICFLOW_THRESHOLD_VEC"/>

<dl_setting var="opticflow.fast9_adaptive" module="computer_vision/opticflow_module" min="0" step="1" max="1" values="TRUE|FALSE" shortname="fast9_adaptive" param="OPTICFLOW_FAST9_ADAPTIVE"/>
<dl_setting var="opticflow.fast9_threshold" module="computer_vision/opticflow_module" min="0" step="1" max="255" shortname="fast9_threshold" param="OPTICFLOW_FAST9_THRESHOLD"/>
<dl_setting var="opticflow.fast9_min_distance" module="computer_vision/opticflow_module" min="0" step="1" max="500" shortname="fast9_min_distance" param="OPTICFLOW_FAST9_MIN_DISTANCE"/>
</dl_settings>
</dl_settings>
</settings>

<header>
<file name="opticflow_module.h"/>
</header>

<init fun="opticflow_module_init()"/>

<periodic fun="opticflow_module_run()" start="opticflow_module_start()" stop="opticflow_module_stop()" autorun="TRUE"/>

<makefile target="ap">
<define name="ARDRONE_VIDEO_PORT" value="2002" />
<!-- Include the needed Computer Vision files -->
<define name="modules/computer_vision" type="include"/>
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
<file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/lib/encoding"/>
<file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>

<!-- The optical flow module (calculator+stabilization) -->
<file name="opticflow_module.c"/>
<file name="opticflow_thread.c" dir="modules/computer_vision/opticflow"/>
<file name="visual_estimator.c" dir="modules/computer_vision/opticflow"/>
<file name="hover_stabilization.c" dir="modules/computer_vision/opticflow"/>
<file name="optic_flow_int.c" dir="modules/computer_vision/cv/opticflow"/>
<file name="fastRosten.c" dir="modules/computer_vision/cv/opticflow/fast9"/>
<file name="trig.c" dir="modules/computer_vision/cv"/>
<file name="framerate.c" dir="modules/computer_vision/cv"/>
<file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/cv/encoding"/>
<file name="socket.c" dir="modules/computer_vision/lib/udp"/>
<file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>
<define name="modules/computer_vision/cv" type="include"/>
<define name="modules/computer_vision/lib" type="include"/>
<define name="pthread" type="raw"/>
<define name="__USE_GNU"/>
<flag name="LDFLAGS" value="pthread"/>
<flag name="LDFLAGS" value="lrt"/>
<flag name="LDFLAGS" value="static"/>
<file name="opticflow_calculator.c" dir="modules/computer_vision/opticflow"/>
<file name="stabilization_opticflow.c" dir="modules/computer_vision/opticflow"/>

<!-- Main vision calculations -->
<file name="fast_rosten.c" dir="modules/computer_vision/lib/vision"/>
<file name="lucas_kanade.c" dir="modules/computer_vision/lib/vision"/>

<raw>
VIEWVIDEO_DEV ?= UDP1
VIEWVIDEO_HOST ?= $(MODEM_HOST)
VIEWVIDEO_PORT_OUT ?= 5000
VIEWVIDEO_PORT_IN ?= 4999
VIEWVIDEO_BROADCAST ?= $(MODEM_BROADCAST)
VIEWVIDEO_DEV_LOWER = $(shell echo $(VIEWVIDEO_DEV) | tr A-Z a-z)

VIEWVID_G_CFLAGS = -DVIEWVIDEO_HOST=\"$(VIEWVIDEO_HOST)\" -DVIEWVIDEO_PORT_OUT=$(VIEWVIDEO_PORT_OUT)
VIEWVID_CFLAGS = -DUSE_$(VIEWVIDEO_DEV) -DVIEWVIDEO_DEV=$(VIEWVIDEO_DEV_LOWER)
VIEWVID_CFLAGS += -D$(VIEWVIDEO_DEV)_PORT_OUT=$(VIEWVIDEO_PORT_OUT) -D$(VIEWVIDEO_DEV)_PORT_IN=$(VIEWVIDEO_PORT_IN)
VIEWVID_CFLAGS += -D$(VIEWVIDEO_DEV)_BROADCAST=$(VIEWVIDEO_BROADCAST) -D$(VIEWVIDEO_DEV)_HOST=\"$(VIEWVIDEO_HOST)\"
ifeq ($(VIEWVIDEO_USE_NC),)
ap.CFLAGS += $(VIEWVID_G_CFLAGS) $(VIEWVID_CFLAGS)
else
ap.CFLAGS += $(VIEWVID_G_CFLAGS) -DVIEWVIDEO_USE_NC
endif

ap.CFLAGS += -DGUIDANCE_V_MODE_MODULE_SETTING=GUIDANCE_V_MODE_HOVER
ap.CFLAGS += -DGUIDANCE_H_MODE_MODULE_SETTING=GUIDANCE_H_MODE_MODULE
</raw>
</makefile>

<makefile target="nps">
<file name="viewvideo_nps.c"/>
</makefile>
Expand Down
5 changes: 3 additions & 2 deletions conf/modules/video_rtp_stream.xml
View file
Expand Up @@ -37,8 +37,9 @@

<!-- Include the needed Computer Vision files -->
<define name="modules/computer_vision" type="include"/>
<file name="jpeg.c" dir="modules/computer_vision/cv/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/cv/encoding"/>
<file name="image.c" dir="modules/computer_vision/lib/vision"/>
<file name="jpeg.c" dir="modules/computer_vision/lib/encoding"/>
<file name="rtp.c" dir="modules/computer_vision/lib/encoding"/>
<file name="v4l2.c" dir="modules/computer_vision/lib/v4l"/>

<!-- Define the network connection to send images over -->
Expand Down
1 change: 1 addition & 0 deletions conf/telemetry/default_ardrone.xml
View file
Expand Up @@ -21,6 +21,7 @@
<message name="UART_ERRORS" period="3.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="STATE_FILTER_STATUS" period="3.2"/>
<message name="OPTIC_FLOW_EST" period="0.25"/>
</mode>

<mode name="ppm">
Expand Down
13 changes: 9 additions & 4 deletions sw/airborne/arch/linux/udp_socket.c
View file
Expand Up @@ -40,7 +40,7 @@
/**
* Create UDP socket and bind it.
* @param[out] sock pointer to already allocated UdpSocket struct
* @param[in] host hostname/address
* @param[in] host ip address or hostname (hostname not possible if static linking)
* @param[in] port_out output port
* @param[in] port_in input port (set to < 0 to disable)
* @param[in] broadcast if TRUE enable broadcasting
Expand All @@ -52,6 +52,7 @@ int udp_socket_create(struct UdpSocket *sock, char *host, int port_out, int port
return -1;
}

#ifndef LINUX_LINK_STATIC
/* try to convert host ipv4 address to binary format */
struct in_addr host_ip;
if (!inet_aton(host, &host_ip)) {
Expand All @@ -66,11 +67,11 @@ int udp_socket_create(struct UdpSocket *sock, char *host, int port_out, int port
if (hp->h_addrtype == AF_INET && hp->h_length == 4) {
/* simply use first address */
memcpy(&host_ip.s_addr, hp->h_addr_list[0], hp->h_length);
}
else {
} else {
return -1;
}
}
#endif

// Create the socket with the correct protocl
sock->sockfd = socket(PF_INET, SOCK_DGRAM, 0);
Expand All @@ -96,7 +97,11 @@ int udp_socket_create(struct UdpSocket *sock, char *host, int port_out, int port
// set the output/destination address for use in sendto later
sock->addr_out.sin_family = PF_INET;
sock->addr_out.sin_port = htons(port_out);
#ifndef LINUX_LINK_STATIC
sock->addr_out.sin_addr.s_addr = host_ip.s_addr;
#else
sock->addr_out.sin_addr.s_addr = inet_addr(host);
#endif
return 0;
}

Expand All @@ -114,7 +119,7 @@ int udp_socket_send(struct UdpSocket *sock, uint8_t *buffer, uint16_t len)
}

ssize_t bytes_sent = sendto(sock->sockfd, buffer, len, 0,
(struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out));
(struct sockaddr *)&sock->addr_out, sizeof(sock->addr_out));
if (bytes_sent != len) {
TRACE(TRACE_ERROR, "error sending to sock %d (%d)\n", (int)bytes_sent, strerror(errno));
}
Expand Down
5 changes: 2 additions & 3 deletions sw/airborne/boards/ardrone/navdata.c
View file
Expand Up @@ -284,10 +284,10 @@ static void *navdata_read(void *data __attribute__((unused)))
if (pint != NULL) {
memmove(navdata_buffer, pint, NAVDATA_PACKET_SIZE - (pint - navdata_buffer));
buffer_idx = pint - navdata_buffer;
fprintf(stderr, "[navdata] sync error, startbyte not found, resetting...\n");
} else {
buffer_idx = 0;
}
fprintf(stderr, "[navdata] sync error, startbyte not found, resetting...\n");
continue;
}

Expand Down Expand Up @@ -379,8 +379,7 @@ void navdata_update()

navdata.imu_available = TRUE;
navdata.packetsRead++;
}
else {
} else {
// no new packet available, still unlock mutex again
pthread_mutex_unlock(&navdata_mutex);
}
Expand Down
33 changes: 0 additions & 33 deletions sw/airborne/boards/bebop/actuators.c
View file
Expand Up @@ -144,36 +144,3 @@ static uint8_t actuators_bebop_checksum(uint8_t *bytes, uint8_t size)

return checksum;
}

/*static void actuators_bebop_saturate(void) {
// Find the lowest and highest commands
int32_t max_cmd = 9000; // Should be gotton from airframe file per motor
int32_t min_cmd = 3000; // Should be gotton from airframe file per motor
for(int i = 0; i < 4; i++) {
if(actuators_bebop.rpm_ref[i] > max_cmd)
max_cmd = actuators_bebop.rpm_ref[i];
if(actuators_bebop.rpm_ref[i] < min_cmd)
min_cmd = actuators_bebop.rpm_ref[i];
}
// Find the maximum motor command (Saturated motor or either MOTOR_MIXING_MAX_MOTOR)
int32_t max_motor = 9000;
for(int i = 0; i < 4; i++) {
if(actuators_bebop.rpm_obs[i] & (1<<15) && max_cmd > (actuators_bebop.rpm_obs[i] & ~(1<<15)))
max_motor = actuators_bebop.rpm_obs[i] & ~(1<<15);
}
// Saturate the offsets
if(max_cmd > max_motor) {
int32_t saturation_offset = 9000 - max_cmd;
for(int i = 0; i < 4; i++)
actuators_bebop.rpm_ref[i] += saturation_offset;
motor_mixing.nb_saturation++;
}
else if(min_cmd < 3000) {
int32_t saturation_offset = 3000 - min_cmd;
for(int i = 0; i < 4; i++)
actuators_bebop.rpm_ref[i] += saturation_offset;
motor_mixing.nb_saturation++;
}
}*/

0 comments on commit ea58643

Please sign in to comment.