Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[vision] Opticflow rewrite #1143

Merged
merged 17 commits into from
Apr 9, 2015
Merged

[vision] Opticflow rewrite #1143

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
17 commits
Select commit Hold shift + click to select a range
0e0d1be
[vision] Opticflow cleanup
fvantienen Mar 11, 2015
9be3226
[vision] Move folders and add general image struct
fvantienen Mar 12, 2015
b5a2500
[vision] More cleanup but still needs testing
fvantienen Mar 12, 2015
715b925
[vision] Optic flow debugging and small fixes
fvantienen Mar 12, 2015
54145a2
[vision] Better FAST9 corner detection
fvantienen Mar 12, 2015
263b9ee
[vision] Optical flow rewrite (still needs testing)
fvantienen Mar 15, 2015
da7f29c
[vision] Add some more documentation
fvantienen Mar 15, 2015
10d7ce4
[vision] Update some stuff
fvantienen Mar 16, 2015
712264c
[vision] Fix optic flow calculations
fvantienen Mar 22, 2015
e8895ae
[linux] Static link LIBC library
fvantienen Mar 22, 2015
ee83a5f
[vision] Add image switch function
fvantienen Mar 23, 2015
f1b30a3
[vision] Fix some comments
fvantienen Mar 23, 2015
38f7c98
[vision] Make optic flow device adjustable
fvantienen Mar 31, 2015
5f49f54
[vision] Add more settings to optical flow
fvantienen Mar 31, 2015
abc2859
[vision] Added more documentation about optical flow
fvantienen Mar 31, 2015
0f7dc6b
[vision] Small fixes and added periodic telemetry
fvantienen Apr 6, 2015
dea8152
[vision] Fix code style formatting
fvantienen Apr 6, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
6 changes: 3 additions & 3 deletions conf/Makefile.arm-linux
View file
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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
Open in desktop
Original file line number Diff line number Diff line change
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 */
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

make a define to enable/disable this only for the ardrone

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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -292,10 +292,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 @@ -387,8 +387,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
Open in desktop
Original file line number Diff line number Diff line change
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++;
}
}*/