mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-09 22:49:53 +08:00
Merge pull request #1062 from paparazzi/opticflow
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
@@ -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="{"nw_motor", "ne_motor", "se_motor", "sw_motor"}"/>
|
||||
<define name="JSBSIM_MODEL" value=""simple_ardrone2""/>
|
||||
<define name="SENSORS_PARAMS" value=""nps_sensors_params_ardrone2.h""/>
|
||||
</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>
|
||||
@@ -318,6 +318,17 @@
|
||||
settings_modules="modules/meteo_france_DAQ.xml"
|
||||
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
|
||||
name="ardrone2_raw"
|
||||
ac_id="201"
|
||||
|
||||
+25
-3
@@ -1972,8 +1972,30 @@
|
||||
<field name="t" type="uint32"/>
|
||||
</message>
|
||||
|
||||
<!--228 is free -->
|
||||
<!--229 is free -->
|
||||
<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="AHRS_ARDRONE2" id="230">
|
||||
<field name="state" type="uint32" />
|
||||
@@ -1997,7 +2019,7 @@
|
||||
<field name="rc_status" type="uint8" values="OK|LOST|REALLY_LOST"/>
|
||||
<field name="frame_rate" type="uint8" unit="Hz"/>
|
||||
<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_motors_on" type="uint8" values="MOTORS_OFF|MOTORS_ON"/>
|
||||
<field name="ap_h_mode" type="uint8" values="KILL|RATE|ATTITUDE|HOVER|NAV|CF"/>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -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>
|
||||
|
||||
@@ -423,6 +423,11 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
|
||||
case AP_MODE_NAV:
|
||||
guidance_h_mode_changed(GUIDANCE_H_MODE_NAV);
|
||||
break;
|
||||
case AP_MODE_MODULE:
|
||||
#ifdef GUIDANCE_H_MODE_MODULE_SETTING
|
||||
guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE_SETTING);
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -464,6 +469,11 @@ void autopilot_set_mode(uint8_t new_autopilot_mode)
|
||||
case AP_MODE_NAV:
|
||||
guidance_v_mode_changed(GUIDANCE_V_MODE_NAV);
|
||||
break;
|
||||
case AP_MODE_MODULE:
|
||||
#ifdef GUIDANCE_V_MODE_MODULE_SETTING
|
||||
guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE_SETTING);
|
||||
#endif
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -50,6 +50,7 @@
|
||||
#define AP_MODE_RC_DIRECT 14 // Safety Pilot Direct Commands for helicopter direct control
|
||||
#define AP_MODE_CARE_FREE_DIRECT 15
|
||||
#define AP_MODE_FORWARD 16
|
||||
#define AP_MODE_MODULE 17
|
||||
|
||||
extern uint8_t autopilot_mode;
|
||||
extern uint8_t autopilot_mode_auto2;
|
||||
|
||||
@@ -27,6 +27,7 @@
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#include "firmwares/rotorcraft/guidance/guidance_h.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_module.h"
|
||||
#include "firmwares/rotorcraft/stabilization.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
|
||||
#include "firmwares/rotorcraft/navigation.h"
|
||||
@@ -252,6 +253,12 @@ void guidance_h_mode_changed(uint8_t new_mode)
|
||||
stabilization_attitude_enter();
|
||||
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:
|
||||
guidance_h_nav_enter();
|
||||
#if NO_ATTITUDE_RESET_ON_MODE_CHANGE
|
||||
@@ -304,6 +311,12 @@ void guidance_h_read_rc(bool_t in_flight)
|
||||
#endif
|
||||
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:
|
||||
if (radio_control.status == RC_OK) {
|
||||
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);
|
||||
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:
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -57,6 +57,7 @@
|
||||
#define GUIDANCE_H_MODE_RC_DIRECT 5
|
||||
#define GUIDANCE_H_MODE_CARE_FREE 6
|
||||
#define GUIDANCE_H_MODE_FORWARD 7
|
||||
#define GUIDANCE_H_MODE_MODULE 8
|
||||
|
||||
|
||||
extern uint8_t guidance_h_mode;
|
||||
|
||||
@@ -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_ */
|
||||
@@ -26,6 +26,7 @@
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_v.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_module.h"
|
||||
|
||||
#include "subsystems/radio_control.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);
|
||||
break;
|
||||
|
||||
#if GUIDANCE_V_MODE_MODULE_SETTING == GUIDANCE_V_MODE_MODULE
|
||||
case GUIDANCE_V_MODE_MODULE:
|
||||
guidance_v_module_enter();
|
||||
break;
|
||||
#endif
|
||||
|
||||
default:
|
||||
break;
|
||||
|
||||
@@ -307,6 +314,12 @@ void guidance_v_run(bool_t in_flight)
|
||||
stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t;
|
||||
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: {
|
||||
if (vertical_mode == VERTICAL_MODE_ALT) {
|
||||
guidance_v_z_sp = -nav_flight_altitude;
|
||||
|
||||
@@ -38,6 +38,7 @@
|
||||
#define GUIDANCE_V_MODE_CLIMB 3
|
||||
#define GUIDANCE_V_MODE_HOVER 4
|
||||
#define GUIDANCE_V_MODE_NAV 5
|
||||
#define GUIDANCE_V_MODE_MODULE 6
|
||||
|
||||
extern uint8_t guidance_v_mode;
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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);
|
||||
@@ -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.
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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 */
|
||||
@@ -23,26 +23,39 @@
|
||||
#include <stdint.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)
|
||||
{
|
||||
uint8_t *source = input->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 x = 0; x < output->w; x += 2) {
|
||||
// YUYV
|
||||
*dest++ = *source++; // U
|
||||
*dest++ = *source++; // Y
|
||||
// now skip 3 pixels
|
||||
if (pixelskip) { source += (pixelskip + 1) * 2; }
|
||||
*dest++ = *source++; // U
|
||||
*dest++ = *source++; // V
|
||||
if (pixelskip) { source += (pixelskip - 1) * 2; }
|
||||
source += pixelskip;
|
||||
*dest++ = *source++; // Y
|
||||
source += pixelskip;
|
||||
}
|
||||
// skip 3 rows
|
||||
if (pixelskip) { source += pixelskip * input->w * 2; }
|
||||
// read 1 in every 'downsample' rows, so skip (downsample-1) rows after reading the first
|
||||
source += (downsample-1) * input->w * 2;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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);
|
||||
@@ -22,7 +22,7 @@
|
||||
#ifndef _VIDEO_H
|
||||
#define _VIDEO_H
|
||||
|
||||
#include "../../cv/image.h"
|
||||
#include "modules/computer_vision/cv/image.h"
|
||||
|
||||
struct buffer_struct {
|
||||
void *buf;
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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_ */
|
||||
@@ -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
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
@@ -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 */
|
||||
@@ -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();
|
||||
}
|
||||
@@ -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 */
|
||||
@@ -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) {}
|
||||
|
||||
|
||||
@@ -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_ */
|
||||
@@ -1266,7 +1266,7 @@ let listen_flight_params = fun geomap auto_center_new_ac alert alt_graph ->
|
||||
let color =
|
||||
match ap_mode with
|
||||
"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
|
||||
| _ -> alert_color in
|
||||
ac.strip#set_color "AP" color;
|
||||
|
||||
@@ -4,7 +4,7 @@ let hostname = ref "localhost"
|
||||
|
||||
(** FIXME: Should be read from messages.xml *)
|
||||
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 gaz_modes = [|"MANUAL";"THROTTLE";"CLIMB";"ALT"|]
|
||||
let lat_modes = [|"MANUAL";"ROLL_RATE";"ROLL";"COURSE"|]
|
||||
|
||||
Reference in New Issue
Block a user