Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 03:27:33 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

Merge pull request #1062 from paparazzi/opticflow

Browse Source 
Add cv_opticflow.xml module.
Used to for hover stabilization on an ARDrone2.

Also adds AP_MODE_MODULE to make it easier to add extra "external" control loops.
This commit is contained in:
Felix Ruess
2015-02-05 22:04:32 +01:00
parent 9dd169c09c 5fd3808a39
commit 17b200a59e
36 changed files with 9914 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ardrone2_opticflow_hover.xml
+230
View File
@@ -0,0 +1,230 @@
<!DOCTYPE airframe SYSTEM "airframe.dtd">
<airframe name="ardrone2_raw">
<firmware name="rotorcraft">
<target name="ap" board="ardrone2_raw">
<subsystem name="telemetry" type="transparent_udp"/>
<subsystem name="radio_control" type="datalink"/>
</target>
<!--target name="nps" board="pc">
<subsystem name="fdm" type="jsbsim"/>
<subsystem name="radio_control" type="ppm"/>
</target-->
<define name="USE_SONAR" value="TRUE"/>
<!-- Subsystem section -->
<subsystem name="motor_mixing"/>
<subsystem name="actuators" type="ardrone2"/>
<subsystem name="imu" type="ardrone2"/>
<subsystem name="gps" type="ublox"/>
<subsystem name="stabilization" type="int_quat"/>
<subsystem name="ahrs" type="int_cmpl_quat"/>
<subsystem name="ins" type="extended"/>
</firmware>
<modules main_freq="512">
<load name="gps_ubx_ucenter.xml"/>
<load name="send_imu_mag_current.xml"/>
<load name="file_logger.xml"/>
<load name="cv_opticflow.xml"/>
</modules>
<commands>
<axis name="PITCH" failsafe_value="0"/>
<axis name="ROLL" failsafe_value="0"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="3000"/>
</commands>
<servos driver="Default">
<servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
<servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
<servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
</servos>
<section name="MIXING" prefix="MOTOR_MIXING_">
<define name="TRIM_ROLL" value="0"/>
<define name="TRIM_PITCH" value="0"/>
<define name="TRIM_YAW" value="0"/>
<define name="NB_MOTOR" value="4"/>
<define name="SCALE" value="255"/>
<!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
<define name="ROLL_COEF" value="{ 256, -256, -256, 256 }"/>
<define name="PITCH_COEF" value="{ 256, 256, -256, -256 }"/>
<define name="YAW_COEF" value="{ -256, 256, -256, 256 }"/>
<define name="THRUST_COEF" value="{ 256, 256, 256, 256 }"/>
</section>
<command_laws>
<call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
<set servo="TOP_LEFT" value="motor_mixing.commands[0]"/>
<set servo="TOP_RIGHT" value="motor_mixing.commands[1]"/>
<set servo="BOTTOM_RIGHT" value="motor_mixing.commands[2]"/>
<set servo="BOTTOM_LEFT" value="motor_mixing.commands[3]"/>
</command_laws>
<section name="VISION" prefix="VISION_">
<define name="HOVER" value="FALSE"/>
<define name="PHI_PGAIN" value="500"/>
<define name="PHI_IGAIN" value="10"/>
<define name="THETA_PGAIN" value="500"/>
<define name="THETA_IGAIN" value="10"/>
<define name="DESIRED_VX" value="0"/>
<define name="DESIRED_VY" value="0"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Accelero -->
<define name="ACCEL_X_NEUTRAL" value="2048"/>
<define name="ACCEL_Y_NEUTRAL" value="2048"/>
<define name="ACCEL_Z_NEUTRAL" value="2048"/>
<!-- Magneto calibration -->
<define name="MAG_X_NEUTRAL" value="0"/>
<define name="MAG_Y_NEUTRAL" value="0"/>
<define name="MAG_Z_NEUTRAL" value="-180"/>
<define name="MAG_X_SENS" value="16." integer="16"/>
<define name="MAG_Y_SENS" value="16." integer="16"/>
<define name="MAG_Z_SENS" value="16." integer="16"/>
<!-- Magneto current calibration -->
<define name="MAG_X_CURRENT_COEF" value="0.0"/>
<define name="MAG_Y_CURRENT_COEF" value="0.0"/>
<define name="MAG_Z_CURRENT_COEF" value="0.0"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<!-- local magnetic field -->
<!-- http://wiki.paparazziuav.org/wiki/Subsystem/ahrs#Local_Magnetic_Field -->
<section name="AHRS" prefix="AHRS_">
<!-- Toulouse -->
<define name="H_X" value="0.513081"/>
<define name="H_Y" value="-0.00242783"/>
<define name="H_Z" value="0.858336"/>
<!-- Delft -->
<!--define name="H_X" value="0.3892503"/>
<define name="H_Y" value="0.0017972"/>
<define name="H_Z" value="0.9211303"/ -->
</section>
<section name="INS" prefix="INS_">
<define name="SONAR_MAX_RANGE" value="2.2"/>
<define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
<!-- setpoints -->
<define name="SP_MAX_P" value="10000"/>
<define name="SP_MAX_Q" value="10000"/>
<define name="SP_MAX_R" value="10000"/>
<define name="DEADBAND_P" value="20"/>
<define name="DEADBAND_Q" value="20"/>
<define name="DEADBAND_R" value="200"/>
<define name="REF_TAU" value="4"/>
<!-- feedback -->
<define name="GAIN_P" value="400"/>
<define name="GAIN_Q" value="400"/>
<define name="GAIN_R" value="350"/>
<define name="IGAIN_P" value="75"/>
<define name="IGAIN_Q" value="75"/>
<define name="IGAIN_R" value="50"/>
<!-- feedforward -->
<define name="DDGAIN_P" value="300"/>
<define name="DDGAIN_Q" value="300"/>
<define name="DDGAIN_R" value="300"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="45" unit="deg"/>
<define name="SP_MAX_THETA" value="45" unit="deg"/>
<define name="SP_MAX_R" value="600" unit="deg/s"/>
<define name="DEADBAND_A" value="0"/>
<define name="DEADBAND_E" value="0"/>
<define name="DEADBAND_R" value="250"/>
<!-- reference -->
<define name="REF_OMEGA_P" value="450" unit="deg/s"/>
<define name="REF_ZETA_P" value="0.9"/>
<define name="REF_MAX_P" value="600." unit="deg/s"/>
<define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_Q" value="450" unit="deg/s"/>
<define name="REF_ZETA_Q" value="0.9"/>
<define name="REF_MAX_Q" value="600." unit="deg/s"/>
<define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>
<define name="REF_OMEGA_R" value="200" unit="deg/s"/>
<define name="REF_ZETA_R" value="0.9"/>
<define name="REF_MAX_R" value="300." unit="deg/s"/>
<define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>
<!-- feedback -->
<define name="PHI_PGAIN" value="592"/>
<define name="PHI_DGAIN" value="303"/>
<define name="PHI_IGAIN" value="0"/>
<define name="THETA_PGAIN" value="606"/>
<define name="THETA_DGAIN" value="303"/>
<define name="THETA_IGAIN" value="0"/>
<define name="PSI_PGAIN" value="529"/>
<define name="PSI_DGAIN" value="353"/>
<define name="PSI_IGAIN" value="0"/>
<!-- feedforward -->
<define name="PHI_DDGAIN" value="0"/>
<define name="THETA_DDGAIN" value="0"/>
<define name="PSI_DDGAIN" value="52"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="283"/>
<define name="HOVER_KD" value="82"/>
<define name="HOVER_KI" value="13"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.655"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<!-- Good weather -->
<define name="MAX_BANK" value="20" unit="deg"/>
<!-- Bad weather -->
<!-- define name="MAX_BANK" value="32" unit="deg"/ -->
<define name="PGAIN" value="79"/>
<define name="DGAIN" value="100"/>
<define name="IGAIN" value="30"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
<define name="JSBSIM_MODEL" value="&quot;simple_ardrone2&quot;"/>
<define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_ardrone2.h&quot;"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
<define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
<define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
</section>
</airframe>
conf/conf_tests.xml
+11
View File
@@ -318,6 +318,17 @@
settings_modules="modules/meteo_france_DAQ.xml" settings_modules="modules/meteo_france_DAQ.xml"
gui_color="blue" gui_color="blue"
/> />
<aircraft
name="ardrone2_opticflow"
ac_id="2"
airframe="airframes/ardrone2_opticflow_hover.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_ardrone.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml settings/control/stabilization_att_int.xml"
settings_modules="modules/gps_ubx_ucenter.xml modules/cv_opticflow.xml"
gui_color="blue"
/>
<aircraft <aircraft
name="ardrone2_raw" name="ardrone2_raw"
ac_id="201" ac_id="201"
conf/messages.xml
+25 -3
View File
@@ -1972,8 +1972,30 @@
<field name="t" type="uint32"/> <field name="t" type="uint32"/>
</message> </message>
<!--228 is free --> <message name="OF_HOVER" id="228">
<!--229 is free --> <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="AHRS_ARDRONE2" id="230"> <message name="AHRS_ARDRONE2" id="230">
<field name="state" type="uint32" /> <field name="state" type="uint32" />
@@ -1997,7 +2019,7 @@
<field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/> <field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
<field name="frame_rate" type="uint8" unit="Hz"/> <field name="frame_rate" type="uint8" unit="Hz"/>
<field name="gps_status" type="uint8" values="NO_FIX|NA|NA|3Dfix"/> <field name="gps_status" type="uint8" values="NO_FIX|NA|NA|3Dfix"/>
<field name="ap_mode" type="uint8" values="KILL|FAILSAFE|HOME|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE|FORWARD"/> <field name="ap_mode" type="uint8" values="KILL|FAILSAFE|HOME|RATE_DIRECT|ATTITUDE_DIRECT|RATE_RC_CLIMB|ATTITUDE_RC_CLIMB|ATTITUDE_CLIMB|RATE_Z_HOLD|ATTITUDE_Z_HOLD|HOVER_DIRECT|HOVER_CLIMB|HOVER_Z_HOLD|NAV|RC_DIRECT|CARE_FREE|FORWARD|MODULE"/>
<field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/> <field name="ap_in_flight" type="uint8" values="ON_GROUND|IN_FLIGHT"/>
<field name="ap_motors_on" type="uint8" values="MOTORS_OFF|MOTORS_ON"/> <field name="ap_motors_on" type="uint8" values="MOTORS_OFF|MOTORS_ON"/>
<field name="ap_h_mode" type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV|CF"/> <field name="ap_h_mode" type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV|CF"/>
conf/modules/ctrl_module_demo.xml
+30
View File
@@ -0,0 +1,30 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="ctrl_module_demo" dir="ctrl">
<doc>
<description>
Demo Control Module
Rate-controller as module sample
</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="CtrlModDemo">
<dl_setting var="ctrl_module_demo_pr_ff_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="pr_ff"/>
<dl_setting var="ctrl_module_demo_pr_d_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="pr_d"/>
<dl_setting var="ctrl_module_demo_y_ff_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="y_ff"/>
<dl_setting var="ctrl_module_demo_y_d_gain" min="0" step="0.01" max="1" module="ctrl/ctrl_module_demo" shortname="y_d"/>
</dl_settings> </dl_settings>
</settings>
<header>
<file name="ctrl_module_demo.h"/>
</header>
<makefile>
<file name="ctrl_module_demo.c"/>
</makefile>
</module>
conf/modules/cv_opticflow.xml
+75
View File
@@ -0,0 +1,75 @@
<!DOCTYPE module SYSTEM "module.dtd">
<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.
- Sonar is required.
- Controller can hold position
</description>
<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"/>
</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>
</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" />
<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="video.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"/>
</makefile>
<makefile target="nps">
<file name="viewvideo_nps.c"/>
</makefile>
</module>
sw/airborne/firmwares/rotorcraft/autopilot.c
+10
View File
@@ -423,6 +423,11 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
case AP_MODE_NAV: case AP_MODE_NAV:
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV); guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
break; break;
case AP_MODE_MODULE:
#ifdef GUIDANCE_H_MODE_MODULE_SETTING
guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING);
#endif
break;
default: default:
break; break;
} }
@@ -464,6 +469,11 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
case AP_MODE_NAV: case AP_MODE_NAV:
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV); guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
break; break;
case AP_MODE_MODULE:
#ifdef GUIDANCE_V_MODE_MODULE_SETTING
guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING);
#endif
break;
default: default:
break; break;
} }
sw/airborne/firmwares/rotorcraft/autopilot.h
+1
View File
@@ -50,6 +50,7 @@
#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control #define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control
#define AP_MODE_CARE_FREE_DIRECT 15 #define AP_MODE_CARE_FREE_DIRECT 15
#define AP_MODE_FORWARD 16 #define AP_MODE_FORWARD 16
#define AP_MODE_MODULE 17
extern uint8_t autopilot_mode; extern uint8_t autopilot_mode;
extern uint8_t autopilot_mode_auto2; extern uint8_t autopilot_mode_auto2;
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+19
View File
@@ -27,6 +27,7 @@
#include "generated/airframe.h" #include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h" #include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h" #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/navigation.h" #include "firmwares/rotorcraft/navigation.h"
@@ -252,6 +253,12 @@ void guidance_h_mode_changed(uint8_t new_mode)
stabilization_attitude_enter(); stabilization_attitude_enter();
break; break;
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
case GUIDANCE_H_MODE_MODULE:
guidance_h_module_enter();
break;
#endif
case GUIDANCE_H_MODE_NAV: case GUIDANCE_H_MODE_NAV:
guidance_h_nav_enter(); guidance_h_nav_enter();
#if NO_ATTITUDE_RESET_ON_MODE_CHANGE #if NO_ATTITUDE_RESET_ON_MODE_CHANGE
@@ -304,6 +311,12 @@ void guidance_h_read_rc(bool_t in_flight)
#endif #endif
break; break;
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
case GUIDANCE_H_MODE_MODULE:
guidance_h_module_read_rc();
break;
#endif
case GUIDANCE_H_MODE_NAV: case GUIDANCE_H_MODE_NAV:
if (radio_control.status == RC_OK) { if (radio_control.status == RC_OK) {
stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight, FALSE, FALSE); stabilization_attitude_read_rc_setpoint_eulers(&guidance_h_rc_sp, in_flight, FALSE, FALSE);
@@ -387,6 +400,12 @@ void guidance_h_run(bool_t in_flight)
stabilization_attitude_run(in_flight); stabilization_attitude_run(in_flight);
break; break;
#if GUIDANCE_H_MODE_MODULE_SETTING == GUIDANCE_H_MODE_MODULE
case GUIDANCE_H_MODE_MODULE:
guidance_h_module_run(in_flight);
break;
#endif
default: default:
break; break;
} }
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+1
View File
@@ -57,6 +57,7 @@
#define GUIDANCE_H_MODE_RC_DIRECT 5 #define GUIDANCE_H_MODE_RC_DIRECT 5
#define GUIDANCE_H_MODE_CARE_FREE 6 #define GUIDANCE_H_MODE_CARE_FREE 6
#define GUIDANCE_H_MODE_FORWARD 7 #define GUIDANCE_H_MODE_FORWARD 7
#define GUIDANCE_H_MODE_MODULE 8
extern uint8_t guidance_h_mode; extern uint8_t guidance_h_mode;
sw/airborne/firmwares/rotorcraft/guidance/guidance_module.h
+55
View File
@@ -0,0 +1,55 @@
/*
* Copyright (C) 2015
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file firmwares/rotorcraft/guidance/guidance_module.h
* Guidance in a module file.
*
* Implement a custom controller in a module.
* Re-use desired modes:
*
* e.g.: <tt>#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER</tt>
* can be used to only define a horizontal control in the module and use normal z_hold
*
* The guidance that the module implement must be activated with following defines:
*
* a) Implement own Horizontal loops when GUIDANCE_H_MODE_MODULE_SETTING is set to GUIDANCE_H_MODE_MODULE
* One must then implement:
* - void guidance_h_module_enter(void);
* - void guidance_h_module_read_rc(void);
* - void guidance_h_module_run(bool_t in_flight);
*
*
* b) Implement own Vertical loops when GUIDANCE_V_MODE_MODULE_SETTING is set to GUIDANCE_V_MODE_MODULE
* - void guidance_v_module_enter(void);
* - void guidance_v_module_run(bool_t in_flight);
*
* If the module implements both V and H mode, take into account that the H is called first and later V
*
*/
#ifndef GUIDANCE_MODULE_H_
#define GUIDANCE_MODULE_H_
#include "generated/modules.h"
#endif /* GUIDANCE_MODULE_H_ */
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
+13
View File
@@ -26,6 +26,7 @@
#include "generated/airframe.h" #include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h" #include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_module.h"
#include "subsystems/radio_control.h" #include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization.h"
@@ -237,6 +238,12 @@ void guidance_v_mode_changed(uint8_t new_mode)
GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0); GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0);
break; break;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
case GUIDANCE_V_MODE_MODULE:
guidance_v_module_enter();
break;
#endif
default: default:
break; break;
@@ -307,6 +314,12 @@ void guidance_v_run(bool_t in_flight)
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
break; break;
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
case GUIDANCE_V_MODE_MODULE:
guidance_v_module_run(in_flight);
break;
#endif
case GUIDANCE_V_MODE_NAV: { case GUIDANCE_V_MODE_NAV: {
if (vertical_mode == VERTICAL_MODE_ALT) { if (vertical_mode == VERTICAL_MODE_ALT) {
guidance_v_z_sp = -nav_flight_altitude; guidance_v_z_sp = -nav_flight_altitude;
sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h
+1
View File
@@ -38,6 +38,7 @@
#define GUIDANCE_V_MODE_CLIMB 3 #define GUIDANCE_V_MODE_CLIMB 3
#define GUIDANCE_V_MODE_HOVER 4 #define GUIDANCE_V_MODE_HOVER 4
#define GUIDANCE_V_MODE_NAV 5 #define GUIDANCE_V_MODE_NAV 5
#define GUIDANCE_V_MODE_MODULE 6
extern uint8_t guidance_v_mode; extern uint8_t guidance_v_mode;
sw/airborne/modules/computer_vision/cv/framerate.c
+76
View File
@@ -0,0 +1,76 @@
/*
* Copyright (C) 2015 The Paparazzi Community
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/cv/framerate.c
*
*/
#include "std.h"
#include "framerate.h"
// Frame Rate (FPS)
#include <sys/time.h>
// local variables
volatile long timestamp;
struct timeval start_time;
struct timeval end_time;
#define USEC_PER_SEC 1000000L
static long time_elapsed(struct timeval *t1, struct timeval *t2)
{
long sec, usec;
sec = t2->tv_sec - t1->tv_sec;
usec = t2->tv_usec - t1->tv_usec;
if (usec < 0) {
--sec;
usec = usec + USEC_PER_SEC;
}
return sec * USEC_PER_SEC + usec;
}
static void start_timer(void)
{
gettimeofday(&start_time, NULL);
}
static long end_timer(void)
{
gettimeofday(&end_time, NULL);
return time_elapsed(&start_time, &end_time);
}
void framerate_init(void) {
// Frame Rate Initialization
timestamp = 0;
start_timer();
}
float framerate_run(void) {
// FPS
timestamp = end_timer();
float framerate_FPS = (float) 1000000 / (float)timestamp;
// printf("dt = %d, FPS = %f\n",timestamp, FPS);
start_timer();
return framerate_FPS;
}
sw/airborne/modules/computer_vision/cv/framerate.h
+28
View File
@@ -0,0 +1,28 @@
/*
* Copyright (C) 2015 The Paparazzi Community
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/cv/framerate.h
*
*/
void framerate_init(void);
float framerate_run(void);
sw/airborne/modules/computer_vision/cv/opticflow/fast9/LICENSE
+30
View File
@@ -0,0 +1,30 @@
Copyright(c) 2006, 2008 Edward Rosten
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
*Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
*Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and / or other materials provided with the distribution.
*Neither the name of the University of Cambridge nor the names of
its contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES(INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT(INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.c
+7320
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/computer_vision/cv/opticflow/fast9/fastRosten.h
+51
View File
@@ -0,0 +1,51 @@
/*
Copyright (c) 2006, 2008 Edward Rosten
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
*Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
*Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
*Neither the name of the University of Cambridge nor the names of
its contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef FAST_H
#define FAST_H
typedef struct { int x, y; } xyFAST;
typedef unsigned char byte;
int fast9_corner_score(const byte *p, const int pixel[], int bstart);
xyFAST *fast9_detect(const byte *im, int xsize, int ysize, int stride, int b, int *ret_num_corners);
int *fast9_score(const byte *i, int stride, xyFAST *corners, int num_corners, int b);
xyFAST *fast9_detect_nonmax(const byte *im, int xsize, int ysize, int stride, int b, int *ret_num_corners);
xyFAST *nonmax_suppression(const xyFAST *corners, const int *scores, int num_corners, int *ret_num_nonmax);
#endif
sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.c
+523
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/computer_vision/cv/opticflow/optic_flow_int.h
+45
View File
@@ -0,0 +1,45 @@
/*
* Copyright (C) 2014
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/cv/opticflow/optic_flow_int.h
* @brief efficient fixed-point optical-flow
*
*/
#ifndef OPTIC_FLOW_INT_H
#define OPTIC_FLOW_INT_H
void multiplyImages(int *ImA, int *ImB, int *ImC, int width, int height);
void getImageDifference(int *ImA, int *ImB, int *ImC, int width, int height);
void getSubPixel_gray(int *Patch, unsigned char *frame_buf, int center_x, int center_y, int half_window_size,
int subpixel_factor);
void getGradientPatch(int *Patch, int *DX, int *DY, int half_window_size);
int getSumPatch(int *Patch, int size);
int calculateG(int *G, int *DX, int *DY, int half_window_size);
int calculateError(int *ImC, int width, int height);
int opticFlowLK(unsigned char *new_image_buf, unsigned char *old_image_buf, int *p_x, int *p_y, int n_found_points,
int imW, int imH, int *new_x, int *new_y, int *status, int half_window_size, int max_iterations);
void quick_sort(float *a, int n);
void quick_sort_int(int *a, int n);
void CvtYUYV2Gray(unsigned char *grayframe, unsigned char *frame, int imW, int imH);
void OFfilter(float *OFx, float *OFy, float dx, float dy, int count, int OF_FilterType);
#endif /* OPTIC_FLOW_INT_H */
sw/airborne/modules/computer_vision/cv/resize.h
+20 -7
View File
@@ -23,26 +23,39 @@
#include <stdint.h> #include <stdint.h>
#include "image.h" #include "image.h"
/** Simplified high-speed low CPU downsample function without averaging
*
* downsample factor must be 1, 2, 4, 8 ... 2^X
* image of typ UYVY expected. Only one color UV per 2 pixels
*
* we keep the UV color of the first pixel pair
* and sample the intensity evenly 1-3-5-7-... or 1-5-9-...
*
* input: u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ...
* downsample=1 u1y1 v1y2 u3y3 v3y4 u5y5 v5y6 u7y7 v7y8 ...
* downsample=2 u1y1v1 (skip2) y3 (skip2) u5y5v5 (skip2 y7 (skip2) ...
* downsample=4 u1y1v1 (skip6) y5 (skip6) ...
*/
inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample); inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample);
inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample) inline void resize_uyuv(struct img_struct *input, struct img_struct *output, int downsample)
{ {
uint8_t *source = input->buf; uint8_t *source = input->buf;
uint8_t *dest = output->buf; uint8_t *dest = output->buf;
int pixelskip = downsample - 1; int pixelskip = (downsample - 1) * 2;
for (int y = 0; y < output->h; y++) { for (int y = 0; y < output->h; y++) {
for (int x = 0; x < output->w; x += 2) { for (int x = 0; x < output->w; x += 2) {
// YUYV // YUYV
*dest++ = *source++; // U *dest++ = *source++; // U
*dest++ = *source++; // Y *dest++ = *source++; // Y
// now skip 3 pixels
if (pixelskip) { source += (pixelskip + 1) * 2; }
*dest++ = *source++; // U
*dest++ = *source++; // V *dest++ = *source++; // V
if (pixelskip) { source += (pixelskip - 1) * 2; } source += pixelskip;
*dest++ = *source++; // Y
source += pixelskip;
} }
// skip 3 rows // read 1 in every 'downsample' rows, so skip (downsample-1) rows after reading the first
if (pixelskip) { source += pixelskip * input->w * 2; } source += (downsample-1) * input->w * 2;
} }
} }
sw/airborne/modules/computer_vision/cv/trig.c
+159
View File
@@ -0,0 +1,159 @@
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
* Trigonometry
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#include "trig.h"
static int cosine[] = {
10000, 9998, 9994, 9986, 9976, 9962, 9945, 9925, 9903, 9877,
9848, 9816, 9781, 9744, 9703, 9659, 9613, 9563, 9511, 9455,
9397, 9336, 9272, 9205, 9135, 9063, 8988, 8910, 8829, 8746,
8660, 8572, 8480, 8387, 8290, 8192, 8090, 7986, 7880, 7771,
7660, 7547, 7431, 7314, 7193, 7071, 6947, 6820, 6691, 6561,
6428, 6293, 6157, 6018, 5878, 5736, 5592, 5446, 5299, 5150,
5000, 4848, 4695, 4540, 4384, 4226, 4067, 3907, 3746, 3584,
3420, 3256, 3090, 2924, 2756, 2588, 2419, 2250, 2079, 1908,
1736, 1564, 1392, 1219, 1045, 872, 698, 523, 349, 175,
0
};
int sin_zelf(int ix)
{
while (ix < 0) {
ix = ix + 360;
}
while (ix >= 360) {
ix = ix - 360;
}
if (ix < 90) { return cosine[90 - ix] / 10; }
if (ix < 180) { return cosine[ix - 90] / 10; }
if (ix < 270) { return -cosine[270 - ix] / 10; }
if (ix < 360) { return -cosine[ix - 270] / 10; }
return 0;
}
int cos_zelf(int ix)
{
while (ix < 0) {
ix = ix + 360;
}
while (ix >= 360) {
ix = ix - 360;
}
if (ix < 90) { return cosine[ix] / 10; }
if (ix < 180) { return -cosine[180 - ix] / 10; }
if (ix < 270) { return -cosine[ix - 180] / 10; }
if (ix < 360) { return cosine[360 - ix] / 10; }
return 0;
}
int tan_zelf(int ix)
{
while (ix < 0) {
ix = ix + 360;
}
while (ix >= 360) {
ix = ix - 360;
}
if (ix == 90) { return 9999; }
if (ix == 270) { return -9999; }
if (ix < 90) { return (1000 * cosine[90 - ix]) / cosine[ix]; }
if (ix < 180) { return -(1000 * cosine[ix - 90]) / cosine[180 - ix]; }
if (ix < 270) { return (1000 * cosine[270 - ix]) / cosine[ix - 180]; }
if (ix < 360) { return -(1000 * cosine[ix - 270]) / cosine[360 - ix]; }
return 0;
}
int asin_zelf(int y, int hyp)
{
int quot, sgn, ix;
if ((y > hyp) || (y == 0)) {
return 0;
}
sgn = hyp * y;
if (hyp < 0) {
hyp = -hyp;
}
if (y < 0) {
y = -y;
}
quot = (y * 10000) / hyp;
if (quot > 9999) {
quot = 9999;
}
for (ix = 0; ix < 90; ix++)
if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) {
break;
}
if (sgn < 0) {
return -(90 - ix);
} else {
return 90 - ix;
}
}
int acos_zelf(int x, int hyp)
{
int quot, sgn, ix;
if (x > hyp) {
return 0;
}
if (x == 0) {
if (hyp < 0) {
return -90;
} else {
return 90;
}
return 0;
}
sgn = hyp * x;
if (hyp < 0) {
hyp = -hyp;
}
if (x < 0) {
x = -x;
}
quot = (x * 10000) / hyp;
if (quot > 9999) {
quot = 9999;
}
for (ix = 0; ix < 90; ix++)
if ((quot < cosine[ix]) && (quot >= cosine[ix + 1])) {
break;
}
if (sgn < 0) {
return -ix;
} else {
return ix;
}
}
//atan_zelf(y/x) in degrees
int atan_zelf(int y, int x)
{
int angle, flip, t, xy;
if (x < 0) { x = -x; }
if (y < 0) { y = -y; }
flip = 0;
if (x < y) { flip = 1; t = x; x = y; y = t; }
if (x == 0) { return 90; }
xy = (y * 1000) / x;
angle = (360 * xy) / (6283 + ((((1764 * xy) / 1000) * xy) / 1000));
if (flip) { angle = 90 - angle; }
return angle;
}
unsigned int isqrt(unsigned int val)
{
unsigned int temp, g = 0, b = 0x8000, bshft = 15;
do {
if (val >= (temp = (((g << 1) + b) << bshft--))) {
g += b;
val -= temp;
}
} while (b >>= 1);
return g;
}
sw/airborne/modules/computer_vision/cv/trig.h
+7
View File
@@ -0,0 +1,7 @@
int cos_zelf(int ix);
int sin_zelf(int);
int tan_zelf(int);
int acos_zelf(int, int);
int asin_zelf(int, int);
int atan_zelf(int, int);
unsigned int isqrt(unsigned int);
sw/airborne/modules/computer_vision/lib/v4l/video.h
+1 -1
View File
@@ -22,7 +22,7 @@
#ifndef _VIDEO_H #ifndef _VIDEO_H
#define _VIDEO_H #define _VIDEO_H
#include "../../cv/image.h" #include "modules/computer_vision/cv/image.h"
struct buffer_struct { struct buffer_struct {
void *buf; void *buf;
sw/airborne/modules/computer_vision/opticflow/hover_stabilization.c
+213
View File
@@ -0,0 +1,213 @@
/*
* Copyright (C) 2014 Hann Woei Ho
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/opticflow/hover_stabilization.c
* @brief optical-flow based hovering for Parrot AR.Drone 2.0
*
* Control loops for optic flow based hovering.
* Computes setpoint for the lower level attitude stabilization to control horizontal velocity.
*/
// Own Header
#include "hover_stabilization.h"
// Stabilization
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "autopilot.h"
// Downlink
#include "subsystems/datalink/downlink.h"
// Controller Gains
/* error if some gains are negative */
#if (VISION_PHI_PGAIN < 0) || \
(VISION_PHI_IGAIN < 0) || \
(VISION_THETA_PGAIN < 0) || \
(VISION_THETA_IGAIN < 0)
#error "ALL control gains have to be positive!!!"
#endif
bool activate_opticflow_hover;
float vision_desired_vx;
float vision_desired_vy;
int32_t vision_phi_pgain;
int32_t vision_phi_igain;
int32_t vision_theta_pgain;
int32_t vision_theta_igain;
// Controller Commands
struct Int32Eulers cmd_euler;
// Hover Stabilization
float Velx_Int;
float Vely_Int;
float Error_Velx;
float Error_Vely;
#define CMD_OF_SAT 1500 // 40 deg = 2859.1851
unsigned char saturateX = 0, saturateY = 0;
unsigned int set_heading;
#ifndef VISION_HOVER
#define VISION_HOVER TRUE
#endif
#ifndef VISION_PHI_PGAIN
#define VISION_PHI_PGAIN 500.
#endif
#ifndef VISION_PHI_IGAIN
#define VISION_PHI_IGAIN 10.
#endif
#ifndef VISION_THETA_PGAIN
#define VISION_THETA_PGAIN 500.
#endif
#ifndef VISION_THETA_IGAIN
#define VISION_THETA_IGAIN 10.
#endif
#ifndef VISION_DESIRED_VX
#define VISION_DESIRED_VX 0.
#endif
#ifndef VISION_DESIRED_VY
#define VISION_DESIRED_VY 0.
#endif
void run_opticflow_hover(void);
void guidance_h_module_enter(void)
{
// INIT
Velx_Int = 0;
Vely_Int = 0;
// GUIDANCE: Set Hover-z-hold
guidance_v_z_sp = -1;
}
void guidance_h_module_read_rc(void)
{
// Do not read RC
// Setpoint being set by vision
}
void guidance_h_module_run(bool_t in_flight)
{
// Run
// Setpoint being set by vision
stabilization_attitude_run(in_flight);
}
void init_hover_stabilization_onvision()
{
INT_EULERS_ZERO(cmd_euler);
activate_opticflow_hover = VISION_HOVER;
vision_phi_pgain = VISION_PHI_PGAIN;
vision_phi_igain = VISION_PHI_IGAIN;
vision_theta_pgain = VISION_THETA_PGAIN;
vision_theta_igain = VISION_THETA_IGAIN;
vision_desired_vx = VISION_DESIRED_VX;
vision_desired_vy = VISION_DESIRED_VY;
set_heading = 1;
Error_Velx = 0;
Error_Vely = 0;
Velx_Int = 0;
Vely_Int = 0;
}
void run_hover_stabilization_onvision(struct CVresults* vision )
{
struct FloatVect3 V_body;
if (activate_opticflow_hover == TRUE) {
// Compute body velocities from ENU
struct FloatVect3 *vel_ned = (struct FloatVect3*)stateGetSpeedNed_f();
struct FloatQuat *q_n2b = stateGetNedToBodyQuat_f();
float_quat_vmult(&V_body, q_n2b, vel_ned);
}
// *************************************************************************************
// Downlink Message
// *************************************************************************************
DOWNLINK_SEND_OF_HOVER(DefaultChannel, DefaultDevice,
&vision->FPS, &vision->dx_sum, &vision->dy_sum, &vision->OFx, &vision->OFy,
&vision->diff_roll, &vision->diff_pitch,
&vision->Velx, &vision->Vely,
&V_body.x, &V_body.y,
&vision->cam_h, &vision->count);
if (autopilot_mode != AP_MODE_MODULE) {
return;
}
if (vision->flow_count) {
Error_Velx = vision->Velx - vision_desired_vx;
Error_Vely = vision->Vely - vision_desired_vy;
} else {
Error_Velx = 0;
Error_Vely = 0;
}
if (saturateX == 0) {
if (activate_opticflow_hover == TRUE) {
Velx_Int += vision_theta_igain * Error_Velx;
} else {
Velx_Int += vision_theta_igain * V_body.x;
}
}
if (saturateY == 0) {
if (activate_opticflow_hover == TRUE) {
Vely_Int += vision_phi_igain * Error_Vely;
} else {
Vely_Int += vision_phi_igain * V_body.y;
}
}
if (set_heading) {
cmd_euler.psi = stateGetNedToBodyEulers_i()->psi;
set_heading = 0;
}
if (activate_opticflow_hover == TRUE) {
cmd_euler.phi = - (vision_phi_pgain * Error_Vely + Vely_Int);
cmd_euler.theta = (vision_theta_pgain * Error_Velx + Velx_Int);
} else {
cmd_euler.phi = - (vision_phi_pgain * V_body.y + Vely_Int);
cmd_euler.theta = (vision_theta_pgain * V_body.x + Velx_Int);
}
saturateX = 0; saturateY = 0;
if (cmd_euler.phi < -CMD_OF_SAT) {cmd_euler.phi = -CMD_OF_SAT; saturateX = 1;}
else if (cmd_euler.phi > CMD_OF_SAT) {cmd_euler.phi = CMD_OF_SAT; saturateX = 1;}
if (cmd_euler.theta < -CMD_OF_SAT) {cmd_euler.theta = -CMD_OF_SAT; saturateY = 1;}
else if (cmd_euler.theta > CMD_OF_SAT) {cmd_euler.theta = CMD_OF_SAT; saturateY = 1;}
stabilization_attitude_set_rpy_setpoint_i(&cmd_euler);
DOWNLINK_SEND_VISION_STABILIZATION(DefaultChannel, DefaultDevice, &vision->Velx, &vision->Vely, &Velx_Int,
&Vely_Int, &cmd_euler.phi, &cmd_euler.theta);
}
sw/airborne/modules/computer_vision/opticflow/hover_stabilization.h
+60
View File
@@ -0,0 +1,60 @@
/*
* Copyright (C) 2014 Hann Woei Ho
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/opticflow/hover_stabilization.h
* @brief optical-flow based hovering for Parrot AR.Drone 2.0
*
* Control loops for optic flow based hovering.
* Computes setpoint for the lower level attitude stabilization to control horizontal velocity.
*/
#ifndef HOVER_STABILIZATION_H_
#define HOVER_STABILIZATION_H_
#include <std.h>
#include "inter_thread_data.h"
// Controller module
// Vertical loop re-uses Alt-hold
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_HOVER
// Horizontal mode is a specific controller
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
// Implement own Horizontal loops
extern void guidance_h_module_enter(void);
extern void guidance_h_module_read_rc(void);
extern void guidance_h_module_run(bool_t in_flight);
void init_hover_stabilization_onvision(void);
void run_hover_stabilization_onvision(struct CVresults *vision);
extern bool activate_opticflow_hover;
extern float vision_desired_vx;
extern float vision_desired_vy;
extern int32_t vision_phi_pgain;
extern int32_t vision_phi_igain;
extern int32_t vision_theta_pgain;
extern int32_t vision_theta_igain;
#endif /* HOVER_STABILIZATION_H_ */
sw/airborne/modules/computer_vision/opticflow/inter_thread_data.h
+56
View File
@@ -0,0 +1,56 @@
/*
* Copyright (C) 2015 The Paparazzi Community
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/opticflow/inter_thread_data.h
* @brief Inter-thread data structures.
*
* Data structures used to for inter-thread communication via Unix Domain sockets.
*/
#ifndef _INTER_THREAD_DATA_H
#define _INTER_THREAD_DATA_H
/// Data from thread to module
struct CVresults {
int cnt; // Number of processed frames
float Velx; // Velocity as measured by camera
float Vely;
int flow_count;
float cam_h; // Debug parameters
int count;
float OFx, OFy, dx_sum, dy_sum;
float diff_roll;
float diff_pitch;
float FPS;
};
/// Data from module to thread
struct PPRZinfo {
int cnt; // IMU msg counter
float phi; // roll [rad]
float theta; // pitch [rad]
float agl; // height above ground [m]
};
#endif
sw/airborne/modules/computer_vision/opticflow/opticflow_thread.c
+150
View File
@@ -0,0 +1,150 @@
/*
* Copyright (C) 2015 The Paparazzi Community
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/opticflow/opticflow_thread.c
*
*/
// Sockets
#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
#include "opticflow_thread.h"
/////////////////////////////////////////////////////////////////////////
// COMPUTER VISION THREAD
// Video
#include "v4l/video.h"
#include "resize.h"
// Payload Code
#include "visual_estimator.h"
// Downlink Video
//#define DOWNLINK_VIDEO 1
#ifdef DOWNLINK_VIDEO
#include "encoding/jpeg.h"
#include "encoding/rtp.h"
#endif
#include <stdio.h>
#define DEBUG_INFO(X, ...) ;
static volatile enum{RUN,EXIT} computer_vision_thread_command = RUN; /** request to close: set to 1 */
void computervision_thread_request_exit(void) {
computer_vision_thread_command = EXIT;
}
void *computervision_thread_main(void *args)
{
int thread_socket = *(int *) args;
// Local data in/out
struct CVresults vision_results;
struct PPRZinfo autopilot_data;
// Status
computer_vision_thread_command = RUN;
// Video Input
struct vid_struct vid;
/* On ARDrone2:
* video1 = front camera; video2 = bottom camera
*/
vid.device = (char *)"/dev/video2";
vid.w = 320;
vid.h = 240;
vid.n_buffers = 4;
if (video_init(&vid) < 0) {
printf("Error initialising video\n");
return 0;
}
// Video Grabbing
struct img_struct *img_new = video_create_image(&vid);
#ifdef DOWNLINK_VIDEO
// Video Compression
uint8_t *jpegbuf = (uint8_t *)malloc(vid.h * vid.w * 2);
// Network Transmit
struct UdpSocket *vsock;
//#define FMS_UNICAST 0
//#define FMS_BROADCAST 1
vsock = udp_socket("192.168.1.255", 5000, 5001, FMS_BROADCAST);
#endif
// First Apply Settings before init
opticflow_plugin_init(vid.w, vid.h, &vision_results);
while (computer_vision_thread_command == RUN) {
// Wait for a new frame
video_grab_image(&vid, img_new);
// Get most recent State information
int bytes_read = sizeof(autopilot_data);
while (bytes_read == sizeof(autopilot_data))
{
bytes_read = recv(thread_socket, &autopilot_data, sizeof(autopilot_data), MSG_DONTWAIT);
if (bytes_read != sizeof(autopilot_data)) {
if (bytes_read != -1) {
printf("[thread] Failed to read %d bytes PPRZ info from socket.\n",bytes_read);
}
}
}
DEBUG_INFO("[thread] Read # %d\n",autopilot_data.cnt);
// Run Image Processing with image and data and get results
opticflow_plugin_run(img_new->buf, &autopilot_data, &vision_results);
/* Send results to main */
vision_results.cnt++;
int bytes_written = write(thread_socket, &vision_results, sizeof(vision_results));
if (bytes_written != sizeof(vision_results)){
perror("[thread] Failed to write to socket.\n");
}
DEBUG_INFO("[thread] Write # %d, (bytes %d)\n",vision_results.cnt, bytes_written);
#ifdef DOWNLINK_VIDEO
// JPEG encode the image:
uint32_t quality_factor = 10; //20 if no resize,
uint8_t dri_header = 0;
uint32_t image_format = FOUR_TWO_TWO; // format (in jpeg.h)
uint8_t *end = encode_image(small.buf, jpegbuf, quality_factor, image_format, small.w, small.h, dri_header);
uint32_t size = end - (jpegbuf);
printf("Sending an image ...%u\n", size);
send_rtp_frame(vsock, jpegbuf, size, small.w, small.h, 0, quality_factor, dri_header, 0);
#endif
}
printf("Thread Closed\n");
video_close(&vid);
return 0;
}
sw/airborne/modules/computer_vision/opticflow/opticflow_thread.h
+33
View File
@@ -0,0 +1,33 @@
/*
* Copyright (C) 2015 The Paparazzi Community
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/opticflow/opticflow_thread.h
* @brief computer vision thread
*
*/
#ifndef OPTICFLOW_THREAD_H
#define OPTICFLOW_THREAD_H
void *computervision_thread_main(void *args); /* computer vision thread: should be given a pointer to a socketpair as argument */
void computervision_thread_request_exit(void);
#endif
sw/airborne/modules/computer_vision/opticflow/visual_estimator.c
+283
View File
@@ -0,0 +1,283 @@
/*
* Copyright (C) 2014 Hann Woei Ho
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/opticflow/visual_estimator.c
* @brief Estimate velocity from optic flow.
*
* Using sensors from vertical camera and IMU of Parrot AR.Drone 2.0.
*
* Warning: all this code is called form the Vision-Thread: do not access any autopilot data in here.
*/
#include "std.h"
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
// Own Header
#include "visual_estimator.h"
// Computer Vision
#include "opticflow/optic_flow_int.h"
#include "opticflow/fast9/fastRosten.h"
// for FPS
#include "modules/computer_vision/cv/framerate.h"
// Local variables
struct visual_estimator_struct
{
// Image size
unsigned int imgWidth;
unsigned int imgHeight;
// Images
uint8_t *prev_frame;
uint8_t *gray_frame;
uint8_t *prev_gray_frame;
// Initialization
int old_img_init;
// Store previous
float prev_pitch;
float prev_roll;
} visual_estimator;
// ARDrone Vertical Camera Parameters
#define FOV_H 0.67020643276
#define FOV_W 0.89360857702
#define Fx_ARdrone 343.1211
#define Fy_ARdrone 348.5053
// Corner Detection
#define MAX_COUNT 100
// Flow Derotation
#define FLOW_DEROTATION
// Called by plugin
void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results)
{
// Initialize variables
visual_estimator.imgWidth = w;
visual_estimator.imgHeight = h;
visual_estimator.gray_frame = (unsigned char *) calloc(w * h, sizeof(uint8_t));
visual_estimator.prev_frame = (unsigned char *) calloc(w * h * 2, sizeof(uint8_t));
visual_estimator.prev_gray_frame = (unsigned char *) calloc(w * h, sizeof(uint8_t));
visual_estimator.old_img_init = 1;
visual_estimator.prev_pitch = 0.0;
visual_estimator.prev_roll = 0.0;
results->OFx = 0.0;
results->OFy = 0.0;
results->dx_sum = 0.0;
results->dy_sum = 0.0;
results->diff_roll = 0.0;
results->diff_pitch = 0.0;
results->cam_h = 0.0;
results->Velx = 0.0;
results->Vely = 0.0;
results->flow_count = 0;
results->cnt = 0;
results->count = 0;
framerate_init();
}
void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults *results)
{
// Corner Tracking
// Working Variables
int max_count = 25;
int borderx = 24, bordery = 24;
int x[MAX_COUNT], y[MAX_COUNT];
int new_x[MAX_COUNT], new_y[MAX_COUNT];
int status[MAX_COUNT];
int dx[MAX_COUNT], dy[MAX_COUNT];
int w = visual_estimator.imgWidth;
int h = visual_estimator.imgHeight;
// Framerate Measuring
results->FPS = framerate_run();
if (visual_estimator.old_img_init == 1) {
memcpy(visual_estimator.prev_frame, frame, w * h * 2);
CvtYUYV2Gray(visual_estimator.prev_gray_frame, visual_estimator.prev_frame, w, h);
visual_estimator.old_img_init = 0;
}
// *************************************************************************************
// Corner detection
// *************************************************************************************
// FAST corner detection
int fast_threshold = 20;
xyFAST *pnts_fast;
pnts_fast = fast9_detect((const byte *)visual_estimator.prev_gray_frame, w, h, w,
fast_threshold, &results->count);
if (results->count > MAX_COUNT) { results->count = MAX_COUNT; }
for (int i = 0; i < results->count; i++) {
x[i] = pnts_fast[i].x;
y[i] = pnts_fast[i].y;
}
free(pnts_fast);
// Remove neighboring corners
const float min_distance = 3;
float min_distance2 = min_distance * min_distance;
int labelmin[MAX_COUNT];
for (int i = 0; i < results->count; i++) {
for (int j = i + 1; j < results->count; j++) {
// distance squared:
float distance2 = (x[i] - x[j]) * (x[i] - x[j]) + (y[i] - y[j]) * (y[i] - y[j]);
if (distance2 < min_distance2) {
labelmin[i] = 1;
}
}
}
int count_fil = results->count;
for (int i = results->count - 1; i >= 0; i--) {
int remove_point = 0;
if (labelmin[i]) {
remove_point = 1;
}
if (remove_point) {
for (int c = i; c < count_fil - 1; c++) {
x[c] = x[c + 1];
y[c] = y[c + 1];
}
count_fil--;
}
}
if (count_fil > max_count) { count_fil = max_count; }
results->count = count_fil;
// *************************************************************************************
// Corner Tracking
// *************************************************************************************
CvtYUYV2Gray(visual_estimator.gray_frame, frame, w, h);
opticFlowLK(visual_estimator.gray_frame, visual_estimator.prev_gray_frame, x, y,
count_fil, w, h, new_x, new_y, status, 5, 100);
results->flow_count = count_fil;
for (int i = count_fil - 1; i >= 0; i--) {
int remove_point = 1;
if (status[i] && !(new_x[i] < borderx || new_x[i] > (w - 1 - borderx) ||
new_y[i] < bordery || new_y[i] > (h - 1 - bordery))) {
remove_point = 0;
}
if (remove_point) {
for (int c = i; c < results->flow_count - 1; c++) {
x[c] = x[c + 1];
y[c] = y[c + 1];
new_x[c] = new_x[c + 1];
new_y[c] = new_y[c + 1];
}
results->flow_count--;
}
}
results->dx_sum = 0.0;
results->dy_sum = 0.0;
// Optical Flow Computation
for (int i = 0; i < results->flow_count; i++) {
dx[i] = new_x[i] - x[i];
dy[i] = new_y[i] - y[i];
}
// Median Filter
if (results->flow_count) {
quick_sort_int(dx, results->flow_count); // 11
quick_sort_int(dy, results->flow_count); // 11
results->dx_sum = (float) dx[results->flow_count / 2];
results->dy_sum = (float) dy[results->flow_count / 2];
} else {
results->dx_sum = 0.0;
results->dy_sum = 0.0;
}
// Flow Derotation
results->diff_pitch = (info->theta - visual_estimator.prev_pitch) * h / FOV_H;
results->diff_roll = (info->phi - visual_estimator.prev_roll) * w / FOV_W;
visual_estimator.prev_pitch = info->theta;
visual_estimator.prev_roll = info->phi;
float OFx_trans, OFy_trans;
#ifdef FLOW_DEROTATION
if (results->flow_count) {
OFx_trans = results->dx_sum - results->diff_roll;
OFy_trans = results->dy_sum - results->diff_pitch;
if ((OFx_trans <= 0) != (results->dx_sum <= 0)) {
OFx_trans = 0;
OFy_trans = 0;
}
} else {
OFx_trans = results->dx_sum;
OFy_trans = results->dy_sum;
}
#else
OFx_trans = results->dx_sum;
OFy_trans = results->dy_sum;
#endif
// Average Filter
OFfilter(&results->OFx, &results->OFy, OFx_trans, OFy_trans, results->flow_count, 1);
// Velocity Computation
if (info->agl < 0.01) {
results->cam_h = 0.01;
}
else {
results->cam_h = info->agl;
}
if (results->flow_count) {
results->Velx = results->OFy * results->cam_h * results->FPS / Fy_ARdrone + 0.05;
results->Vely = -results->OFx * results->cam_h * results->FPS / Fx_ARdrone - 0.1;
} else {
results->Velx = 0.0;
results->Vely = 0.0;
}
// *************************************************************************************
// Next Loop Preparation
// *************************************************************************************
memcpy(visual_estimator.prev_frame, frame, w * h * 2);
memcpy(visual_estimator.prev_gray_frame, visual_estimator.gray_frame, w * h);
}
sw/airborne/modules/computer_vision/opticflow/visual_estimator.h
+41
View File
@@ -0,0 +1,41 @@
/*
* Copyright (C) 2014 Hann Woei Ho
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/opticflow/visual_estimator.h
* @brief Estimate velocity from optic flow.
*
* Using sensors from vertical camera and IMU of Parrot AR.Drone 2.0
*/
#ifndef VISUAL_ESTIMATOR_H
#define VISUAL_ESTIMATOR_H
#include "inter_thread_data.h"
/**
* Initialize visual estimator.
* @param w image width
* @param h image height
*/
void opticflow_plugin_init(unsigned int w, unsigned int h, struct CVresults *results);
void opticflow_plugin_run(unsigned char *frame, struct PPRZinfo* info, struct CVresults* results);
#endif /* VISUAL_ESTIMATOR_H */
sw/airborne/modules/computer_vision/opticflow_module.c
+147
View File
@@ -0,0 +1,147 @@
/*
* Copyright (C) 2014 Hann Woei Ho
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/opticflow_module.c
* @brief optical-flow based hovering for Parrot AR.Drone 2.0
*
* Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
*/
#include "opticflow_module.h"
// Computervision Runs in a thread
#include "opticflow/opticflow_thread.h"
#include "opticflow/inter_thread_data.h"
// Navigate Based On Vision, needed to call init/run_hover_stabilization_onvision
#include "opticflow/hover_stabilization.h"
// Threaded computer vision
#include <pthread.h>
// Sockets
#include <errno.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/socket.h>
int cv_sockets[2];
// Paparazzi Data
#include "state.h"
#include "subsystems/abi.h"
// Downlink
#include "subsystems/datalink/downlink.h"
struct PPRZinfo opticflow_module_data;
/** height above ground level, from ABI
* Used for scale computation, negative value means invalid.
*/
/** default sonar/agl to use in opticflow visual_estimator */
#ifndef OPTICFLOW_AGL_ID
#define OPTICFLOW_AGL_ID ABI_BROADCAST
#endif
abi_event agl_ev;
static void agl_cb(uint8_t sender_id, const float *distance);
static void agl_cb(uint8_t sender_id __attribute__((unused)), const float *distance)
{
if (*distance > 0) {
opticflow_module_data.agl = *distance;
}
}
#define DEBUG_INFO(X, ...) ;
void opticflow_module_init(void)
{
// get AGL from sonar via ABI
AbiBindMsgAGL(OPTICFLOW_AGL_ID, &agl_ev, agl_cb);
// Initialize local data
opticflow_module_data.cnt = 0;
opticflow_module_data.phi = 0;
opticflow_module_data.theta = 0;
opticflow_module_data.agl = 0;
// Stabilization Code Initialization
init_hover_stabilization_onvision();
}
void opticflow_module_run(void)
{
// Send Updated data to thread
opticflow_module_data.cnt++;
opticflow_module_data.phi = stateGetNedToBodyEulers_f()->phi;
opticflow_module_data.theta = stateGetNedToBodyEulers_f()->theta;
int bytes_written = write(cv_sockets[0], &opticflow_module_data, sizeof(opticflow_module_data));
if (bytes_written != sizeof(opticflow_module_data)){
printf("[module] Failed to write to socket: written = %d, error=%d.\n",bytes_written, errno);
}
else {
DEBUG_INFO("[module] Write # %d (%d bytes)\n",opticflow_module_data.cnt, bytes_written);
}
// Read Latest Vision Module Results
struct CVresults vision_results;
// Warning: if the vision runs faster than the module, you need to read multiple times
int bytes_read = recv(cv_sockets[0], &vision_results, sizeof(vision_results), MSG_DONTWAIT);
if (bytes_read != sizeof(vision_results)) {
if (bytes_read != -1) {
printf("[module] Failed to read %d bytes: CV results from socket errno=%d.\n",bytes_read, errno);
}
} else {
////////////////////////////////////////////
// Module-Side Code
////////////////////////////////////////////
DEBUG_INFO("[module] Read vision %d\n",vision_results.cnt);
run_hover_stabilization_onvision(&vision_results);
}
}
void opticflow_module_start(void)
{
pthread_t computervision_thread;
if (socketpair(AF_UNIX, SOCK_DGRAM, 0, cv_sockets) == 0) {
////////////////////////////////////////////
// Thread-Side Code
////////////////////////////////////////////
int rc = pthread_create(&computervision_thread, NULL, computervision_thread_main,
&cv_sockets[1]);
if (rc) {
printf("ctl_Init: Return code from pthread_create(mot_thread) is %d\n", rc);
}
}
else {
perror("Could not create socket.\n");
}
}
void opticflow_module_stop(void)
{
computervision_thread_request_exit();
}
sw/airborne/modules/computer_vision/opticflow_module.h
+39
View File
@@ -0,0 +1,39 @@
/*
* Copyright (C) 2014 Hann Woei Ho
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/computer_vision/opticflow_module.h
* @brief optical-flow based hovering for Parrot AR.Drone 2.0
*
* Sensors from vertical camera and IMU of Parrot AR.Drone 2.0
*/
#ifndef OPTICFLOW_MODULE_H
#define OPTICFLOW_MODULE_H
#include "std.h"
// Module functions
extern void opticflow_module_init(void);
extern void opticflow_module_run(void);
extern void opticflow_module_start(void);
extern void opticflow_module_stop(void);
#endif /* OPTICFLOW_MODULE_H */
sw/airborne/modules/ctrl/ctrl_module_demo.c
+104
View File
@@ -0,0 +1,104 @@
/*
* Copyright (C) 2015
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/ctrl/ctrl_module_demo.h
* @brief example empty controller
*
*/
#include "modules/ctrl/ctrl_module_demo.h"
#include "state.h"
#include "subsystems/radio_control.h"
#include "firmwares/rotorcraft/stabilization.h"
struct ctrl_module_demo_struct {
int rc_x;
int rc_y;
int rc_z;
int rc_t;
} ctrl_module_demo;
float ctrl_module_demo_pr_ff_gain = 0.2f; // Pitch/Roll
float ctrl_module_demo_pr_d_gain = 0.1f;
float ctrl_module_demo_y_ff_gain = 0.4f; // Yaw
float ctrl_module_demo_y_d_gain = 0.05f;
void ctrl_module_init(void);
void ctrl_module_run(bool_t in_flight);
void ctrl_module_init(void)
{
ctrl_module_demo.rc_x = 0;
ctrl_module_demo.rc_y = 0;
ctrl_module_demo.rc_z = 0;
ctrl_module_demo.rc_t = 0;
}
// Old-fashened rate control without reference model nor attitude
void ctrl_module_run(bool_t in_flight)
{
if (!in_flight) {
// Reset integrators
stabilization_cmd[COMMAND_ROLL] = 0;
stabilization_cmd[COMMAND_PITCH] = 0;
stabilization_cmd[COMMAND_YAW] = 0;
stabilization_cmd[COMMAND_THRUST] = 0;
} else {
stabilization_cmd[COMMAND_ROLL] = ctrl_module_demo.rc_x * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->p *
ctrl_module_demo_pr_d_gain;
stabilization_cmd[COMMAND_PITCH] = ctrl_module_demo.rc_y * ctrl_module_demo_pr_ff_gain - stateGetBodyRates_i()->q *
ctrl_module_demo_pr_d_gain;
stabilization_cmd[COMMAND_YAW] = ctrl_module_demo.rc_z * ctrl_module_demo_y_ff_gain - stateGetBodyRates_i()->r *
ctrl_module_demo_y_d_gain;
stabilization_cmd[COMMAND_THRUST] = ctrl_module_demo.rc_t;
}
}
////////////////////////////////////////////////////////////////////
// Call our controller
// Implement own Horizontal loops
void guidance_h_module_enter(void)
{
ctrl_module_init();
}
void guidance_h_module_read_rc(void)
{
// -MAX_PPRZ to MAX_PPRZ
ctrl_module_demo.rc_t = radio_control.values[RADIO_THROTTLE];
ctrl_module_demo.rc_x = radio_control.values[RADIO_ROLL];
ctrl_module_demo.rc_y = radio_control.values[RADIO_PITCH];
ctrl_module_demo.rc_z = radio_control.values[RADIO_YAW];
}
void guidance_h_module_run(bool_t in_flight)
{
// Call full inner-/outerloop / horizontal-/vertical controller:
ctrl_module_run(in_flight);
}
// Implement own Horizontal loops
inline void guidance_v_module_enter(void) {}
inline void guidance_v_module_run(UNUSED bool_t in_flight) {}
sw/airborne/modules/ctrl/ctrl_module_demo.h
+55
View File
@@ -0,0 +1,55 @@
/*
* Copyright (C) 2015
*
* This file is part of Paparazzi.
*
* Paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* Paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/ctrl/ctrl_module_demo.c
* @brief example empty controller
*
*/
#ifndef CTRL_MODULE_DEMO_H_
#define CTRL_MODULE_DEMO_H_
#include <std.h>
// Settings
extern float ctrl_module_demo_pr_ff_gain; // Pitch/Roll
extern float ctrl_module_demo_pr_d_gain;
extern float ctrl_module_demo_y_ff_gain; // Yaw
extern float ctrl_module_demo_y_d_gain;
// Demo with own guidance_h
#define GUIDANCE_H_MODE_MODULE_SETTING GUIDANCE_H_MODE_MODULE
// and own guidance_v
#define GUIDANCE_V_MODE_MODULE_SETTING GUIDANCE_V_MODE_MODULE
// Implement own Horizontal loops
extern void guidance_h_module_enter(void);
extern void guidance_h_module_read_rc(void);
extern void guidance_h_module_run(bool_t in_flight);
// Implement own Horizontal loops
extern void guidance_v_module_enter(void);
extern void guidance_v_module_run(bool_t in_flight);
#endif /* HOVER_STABILIZATION_H_ */
sw/ground_segment/cockpit/live.ml
+1 -1
View File
@@ -1266,7 +1266,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
let color = let color =
match ap_mode with match ap_mode with
"AUTO2" | "NAV" -> ok_color "AUTO2" | "NAV" -> ok_color
| "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" -> "#10F0E0" | "AUTO1" | "R_RCC" | "A_RCC" | "ATT_C" | "R_ZH" | "A_ZH" | "HOVER" | "HOV_C" | "H_ZH" | "MODULE" -> "#10F0E0"
| "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" -> warning_color | "MANUAL" | "RATE" | "ATT" | "RC_D" | "CF" | "FWD" -> warning_color
| _ -> alert_color in | _ -> alert_color in
ac.strip#set_color "AP" color; ac.strip#set_color "AP" color;
sw/ground_segment/tmtc/server_globals.ml
+1 -1
View File
@@ -4,7 +4,7 @@ let hostname = ref "localhost"
(** FIXME: Should be read from messages.xml *) (** FIXME: Should be read from messages.xml *)
let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS";"FAIL"|] let fixedwing_ap_modes = [|"MANUAL";"AUTO1";"AUTO2";"HOME";"NOGPS";"FAIL"|]
let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF";"FWD"|] let rotorcraft_ap_modes = [|"KILL";"SAFE";"HOME";"RATE";"ATT";"R_RCC";"A_RCC";"ATT_C";"R_ZH";"A_ZH";"HOVER";"HOV_C";"H_ZH";"NAV";"RC_D";"CF";"FWD";"MODULE"|]
let _AUTO2 = 2 let _AUTO2 = 2
let gaz_modes = [|"MANUAL";"THROTTLE";"CLIMB";"ALT"|] let gaz_modes = [|"MANUAL";"THROTTLE";"CLIMB";"ALT"|]
let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|] let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]