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

Merge branch 'master' into ahrs_refactor

Browse Source 
to get the ABI changes (not forced to pass const pointers)

* master: (99 commits)
  [abi] allow to pass variables by value
  Sign of pitch roll axis changed and yaw fit
  [opticflow] dox
  [opticflow] working variables in a struct
  Added new RC USB type joystick from Hobbyking
  Messages in module instead of thread
  fix thread communication
  convert to thread-communication
  [module] video_usb_logger
  [opticflow] example using unix domain sockets to return thread results
  [opticflow] compute V_body in hover_stabilization
  [python] ivy_msg_interface returns only ac_id and msg
  [python] ivytoredis: publish msg as json
  [python] refactor messages
  [python] messages: fix typo
  [python] messages: PPRZ_HOME
  [fix] fix extra_dl and pprrzlog
  [stm32] use desig_get_unique_id from libopencm3
  [fix] call pprzlog_init if needed (and avoid segfault)
  [message] improve ivy perf by only waiting for ALIVE by default
  ...
This commit is contained in:
Felix Ruess
2015-02-07 16:11:54 +01:00
parent b25b4c1fe3 30df14eb05
commit 12e256dfa7
131 changed files with 11473 additions and 733 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_example.xml
+1 -1
View File
@@ -7,8 +7,8 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml"
gui_color="white"
settings_modules=""
gui_color="white"
/>
<aircraft
name="Bixler"
conf/conf_tests.xml
+23 -12
View File
@@ -7,8 +7,8 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/dummy.xml"
settings="settings/rotorcraft_basic.xml"
gui_color="white"
settings_modules=""
gui_color="white"
/>
<aircraft
name="Bixler"
@@ -65,6 +65,17 @@
settings_modules=""
gui_color="white"
/>
<aircraft
name="JP"
ac_id="40"
airframe="airframes/ENAC/fixed-wing/jp.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
flight_plan="flight_plans/basic.xml"
settings="settings/fixedwing_basic.xml settings/control/ctl_new.xml"
settings_modules="modules/airspeed_adc.xml modules/gps_ubx_ucenter.xml"
gui_color="#ffff7d7d0000"
/>
<aircraft
name="LadyLisa"
ac_id="164"
@@ -307,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"
@@ -406,15 +428,4 @@
settings_modules=""
gui_color="blue"
/>
<aircraft
name="JP"
ac_id="40"
airframe="airframes/ENAC/fixed-wing/jp.xml"
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
flight_plan="flight_plans/basic.xml"
settings="settings/fixedwing_basic.xml settings/control/ctl_new.xml"
settings_modules="modules/airspeed_adc.xml modules/gps_ubx_ucenter.xml"
gui_color="#ffff7d7d0000"
/>
</conf>
conf/firmwares/subsystems/rotorcraft/navigation.makefile
+1
View File
@@ -2,4 +2,5 @@
$(TARGET).CFLAGS += -DUSE_NAVIGATION
$(TARGET).srcs += $(SRC_FIRMWARE)/navigation.c
$(TARGET).srcs += subsystems/navigation/waypoints.c
$(TARGET).srcs += subsystems/navigation/common_flight_plan.c
conf/firmwares/subsystems/fixedwing/current_sensor.makefile → conf/firmwares/subsystems/shared/current_sensor.makefile
View File
conf/firmwares/subsystems/shared/sdlog.makefile
+1 -1
View File
@@ -1,6 +1,6 @@
# Hey Emacs, this is a -*- makefile -*-
sdlog_CFLAGS = -DDOWNLINK
sdlog_CFLAGS = -DDOWNLINK -DUSE_PPRZLOG
sdlog_srcs = subsystems/datalink/downlink.c subsystems/datalink/pprzlog_transport.c
ap.CFLAGS += $(sdlog_CFLAGS)
conf/joystick/hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml
+50
View File
@@ -0,0 +1,50 @@
<!--
Hobbyking 6CH RC Transmitter shaped USB HID joystick Mode 2
http://www.hobbyking.com/hobbyking/store/__20951__Hobbyking_6CH_RC_Flight_Simulator_System_Mode_2_.html
If you want to use this file for the same product but mode 1 the axis index
numbers have to be re-arranged, the rest will be exactly the same.
This RC TX joystick has four axes,2 buttons, one dial, which are labeled on the
RC TX joystick
The button and dial give their output on an **axis** type not button type
The button ""MIX" is not useful and hardwired to mixing and does NOT give
indepenadant output, better remove it from your RC joystick.
The 6CH Gyro switch only has 2 positions, not three :(
We will use it as mode switch none the less between, MANUAL (or ATT) and NAV
So it is a real 6CH USB joystick that just looks like a RC transmitter
If you want to fly your UAS via the joystick add thid to your session:
/home/username/paparazzi/sw/ground_segment/joystick/input2ivy -d 0 -ac yourarfamename hobbyking_usb_rc_6ch_rc_tx_joy_mode_2.xml
Where -d 0 must be -d 1 if you have a laptop with accelometer installed
The basis of steering is the standard signs of aerospace convention
-->
<joystick>
<input>
<axis index="0" name="roll"/>
<axis index="1" name="pitch"/>
<axis index="2" name="throttle"/>
<axis index="3" name="mode"/>
<axis index="4" name="gearpit"/>
<axis index="5" name="yaw"/>
</input>
<messages period="0.0333333">
<message class="datalink" name="RC_4CH" send_always="true">
<field name="throttle" value="Fit(-throttle,-124,126,0,127)" />
<field name="roll" value="roll" />
<field name="pitch" value="pitch" />
<field name="yaw" value="Fit(yaw,-125,125,-127,127)"/>
<field name="mode" value="Fit(mode,-127,126,0,2)"/>
</message>
</messages>
</joystick>
conf/messages.xml
+25 -3
View File
@@ -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"/>
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>
conf/modules/extra_dl.xml
+1
View File
@@ -9,6 +9,7 @@
<header>
<file name="extra_pprz_dl.h"/>
</header>
<init fun="extra_pprz_dl_init()"/>
<event fun="ExtraDatalinkEvent()"/>
<makefile target="ap">
<raw>
conf/modules/nav_bungee_takeoff.xml
+13 -11
View File
@@ -3,18 +3,20 @@
<module name="nav_bungee_takeoff" dir="nav">
<doc>
<description>
Takeoff functions for bungee takeoff.
Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to
launch the plane. After initialized, the plane will follow a line drawn by the position of the plane on initialization and the
position of the bungee (given in the arguments). Once the plane crosses the throttle line, which is perpendicular to the line the plane is following,
and intersects the position of the bungee (plus or minus a fixed distance (TakeOff_Distance in airframe file) from the bungee just in case the bungee doesn't release directly above the bungee) the prop will come on. The plane will then continue to follow the line until it has reached a specific
height (defined in as Takeoff_Height in airframe file) above the bungee waypoint and speed (defined as Takeoff_Speed in the airframe file).
Takeoff functions for bungee takeoff.
Run initialize function when the plane is on the bungee, the bungee is fully extended and you are ready to launch the plane.
After initialized, the plane will follow a line drawn by the position of the plane on initialization and the position of the bungee (given in the arguments).
Once the plane crosses the throttle line, which is perpendicular to the line the plane is following, and intersects the position of the bungee (plus or minus a fixed distance (BUNGEE_TAKEOFF_DISTANCE in airframe file) from the bungee just in case the bungee doesn't release exactly above the bungee) the prop will come on.
The plane will then continue to follow the line until it has reached a specific height (defined in as BUNGEE_TAKEOFF_HEIGHT in airframe file) above the bungee waypoint and airspeed (defined as BUNGEE_TAKEOFF_AIRSPEED in the airframe file). The airspeed limit is only used if USE_AIRSPEED flag is defined or set to true (and assuming the airspeed is then available). It is also possible to specify the pitch angle (BUNGEE_TAKEOFF_PITCH) and the throttle (BUNGEE_TAKEOFF_THROTTLE, between 0 and 1).
</description>
<section name="Takeoff" prefix="Takeoff_">
<define name="Height" value="distance" unit="m" description="Takeoff height"/>
<define name="Speed" value="speed" unit="m/s" description="Procedures ends above this speed (and above Height)"/>
<define name="Distance" value="distance" unit="m" description="After this distance, the throttle is activated (if above MinSpeed)"/>
<define name="MinSpeed" value="speed" unit="m/s" description="Throttle is activated if crossing the line above this speed"/>
<section name="BUNGEE" prefix="BUNGEE_TAKEOFF_">
<define name="HEIGHT" value="distance" unit="m" description="Takeoff height"/>
<define name="AIRSPEED" value="airspeed" unit="m/s" description="Procedures ends above this airspeed (and above HEIGHT). Only works with valid airspeed data and USE_AIRSPEED flag"/>
<define name="DISTANCE" value="distance" unit="m" description="After this distance, the throttle is activated (if above MIN_SPEED)"/>
<define name="MIN_SPEED" value="speed" unit="m/s" description="Throttle is activated if crossing the line above this speed"/>
<define name="PITCH" value="angle" unit="deg" description="Pitch angle during the complete takeoff phase"/>
<define name="THROTTLE" value="throttle" unit="normalized" description="Throttle setpoint (between 0 and 1) used after crossing the throttle line"/>
</section>
</doc>
<header>
conf/modules/video_usb_logger.xml
+24
View File
@@ -0,0 +1,24 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="video_usb_logger" dir="computer_vision">
<doc>
<description>
Log video and pose to USB-stick.
Logs attitude and position to a csv and images to jpeg files (only for linux).
</description>
<define name="VIDEO_USB_LOGGER_PATH" description="Logging path"/>
</doc>
<depends>video_rtp_stream</depends>
<header>
<file name="video_usb_logger.h"/>
</header>
<periodic fun="video_usb_logger_periodic()" start="video_usb_logger_start()" stop="video_usb_logger_stop()" autorun="FALSE"/>
<makefile target="ap">
<file name="video_usb_logger.c"/>
<define name="VIDEO_DOWNSIZE_FACTOR" value="2"/>
<define name="USE_BOTTOM_CAMERA" value="1"/>
<define name="LOG_ON_USB" value="1"/>
<define name="VIDEO_FPS" value="10"/>
</makefile>
</module>
sw/airborne/arch/stm32/usb_ser_hw.c
+2 -28
View File
@@ -35,6 +35,7 @@
#include <libopencm3/usb/usbd.h>
#include <libopencm3/usb/cdc.h>
#include <libopencm3/cm3/scb.h>
#include <libopencm3/stm32/desig.h>
#include "mcu_periph/usb_serial.h"
@@ -61,8 +62,6 @@ bool_t fifo_put(fifo_t *fifo, uint8_t c);
bool_t fifo_get(fifo_t *fifo, uint8_t *pc);
int fifo_avail(fifo_t *fifo);
int fifo_free(fifo_t *fifo);
inline char *get_dev_unique_id(char *serial_no);
usbd_device *my_usbd_dev;
@@ -215,31 +214,6 @@ static const char *usb_strings[] = {
serial_no,
};
/**
* Serial is 96bit so 12bytes so 12 hexa numbers, or 24 decimal + termination character
*/
inline char *get_dev_unique_id(char *s)
{
#if defined STM32F4
volatile uint8_t *unique_id = (volatile uint8_t *)0x1FFF7A10;
#else
volatile uint8_t *unique_id = (volatile uint8_t *)0x1FFFF7E8;
#endif
int i;
// Fetch serial number from chip's unique ID
for (i = 0; i < 24; i += 2) {
s[i] = ((*unique_id >> 4) & 0xF) + '0';
s[i + 1] = (*unique_id++ & 0xF) + '0';
}
for (i = 0; i < 24; i++)
if (s[i] > '9') {
s[i] += 'A' - '9' - 1;
}
// add termination character
s[24] = '\0';
return s;
}
/*
* Buffer to be used for control requests.
@@ -529,7 +503,7 @@ void VCOM_init(void)
rcc_periph_clock_enable(RCC_OTGFS);
/* Get serial number */
get_dev_unique_id(serial_no);
desig_get_unique_id_as_string(serial_no, 25);
/* usb driver init*/
my_usbd_dev = usbd_init(&otgfs_usb_driver, &dev, &config,
sw/airborne/boards/apogee/baro_board.c
+2 -2
View File
@@ -101,9 +101,9 @@ void apogee_baro_event(void)
mpl3115_event(&apogee_baro);
if (apogee_baro.data_available && startup_cnt == 0) {
float pressure = ((float)apogee_baro.pressure / (1 << 2));
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
float temp = apogee_baro.temperature / 16.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp);
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
apogee_baro.data_available = FALSE;
}
}
sw/airborne/boards/ardrone/baro_board.c
+2 -2
View File
@@ -95,13 +95,13 @@ void ardrone_baro_event(void)
if (baro_calibrated) {
// first read temperature because pressure calibration depends on temperature
float temp_deg = 0.1 * baro_apply_calibration_temp(navdata.temperature_pressure);
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp_deg);
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp_deg);
int32_t press_pascal = baro_apply_calibration(navdata.pressure);
#if USE_BARO_MEDIAN_FILTER
press_pascal = update_median_filter(&baro_median, press_pascal);
#endif
float pressure = (float)press_pascal;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
}
navdata_baro_available = FALSE;
}
sw/airborne/boards/ardrone/navdata.c
+1 -1
View File
@@ -550,7 +550,7 @@ void navdata_update()
// Check if there is a new sonar measurement and update the sonar
if (navdata.ultrasound >> 15) {
float sonar_meas = (float)((navdata.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas);
AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas);
}
#endif
sw/airborne/boards/baro_board_ms5611_i2c.c
+2 -2
View File
@@ -98,9 +98,9 @@ void baro_event(void)
if (bb_ms5611.data_available) {
float pressure = (float)bb_ms5611.data.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
float temp = bb_ms5611.data.temperature / 100.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp);
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
bb_ms5611.data_available = FALSE;
#ifdef BARO_LED
sw/airborne/boards/baro_board_ms5611_spi.c
+2 -2
View File
@@ -87,9 +87,9 @@ void baro_event(void)
if (bb_ms5611.data_available) {
float pressure = (float)bb_ms5611.data.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
float temp = bb_ms5611.data.temperature / 100.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp);
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
bb_ms5611.data_available = FALSE;
#ifdef BARO_LED
sw/airborne/boards/booz/baro_board.c
+1 -1
View File
@@ -86,7 +86,7 @@ void baro_periodic(void)
RunOnceEvery(10, { baro_board_calibrate();});
} else {
float pressure = 101325.0 - BOOZ_BARO_SENS * (BOOZ_ANALOG_BARO_THRESHOLD - baro_board.absolute);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
}
}
sw/airborne/boards/hbmini/baro_board.c
+1 -1
View File
@@ -57,7 +57,7 @@ void bmp_baro_event(void)
if (baro_bmp085.data_available) {
float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
baro_bmp085.data_available = FALSE;
#ifdef BARO_LED
RunOnceEvery(10, LED_TOGGLE(BARO_LED));
sw/airborne/boards/lisa_l/baro_board.c
+2 -2
View File
@@ -139,7 +139,7 @@ void lisa_l_baro_event(void)
if (baro_trans.status == I2CTransSuccess) {
int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1];
float pressure = LISA_L_BARO_SENS * (float)tmp;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
}
} else if (baro_board.status == LBS_READING_DIFF &&
baro_trans.status != I2CTransPending) {
@@ -147,7 +147,7 @@ void lisa_l_baro_event(void)
if (baro_trans.status == I2CTransSuccess) {
int16_t tmp = baro_trans.buf[0] << 8 | baro_trans.buf[1];
float diff = LISA_L_DIFF_SENS * (float)tmp;
AbiSendMsgBARO_DIFF(BARO_BOARD_SENDER_ID, &diff);
AbiSendMsgBARO_DIFF(BARO_BOARD_SENDER_ID, diff);
}
}
}
sw/airborne/boards/lisa_m/baro_board.c
+2 -2
View File
@@ -75,9 +75,9 @@ void baro_event(void)
if (baro_bmp085.data_available) {
float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
float temp = baro_bmp085.temperature / 10.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp);
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
baro_bmp085.data_available = FALSE;
#ifdef BARO_LED
RunOnceEvery(10, LED_TOGGLE(BARO_LED));
sw/airborne/boards/navgo/baro_board.c
+1 -1
View File
@@ -86,7 +86,7 @@ void navgo_baro_event(void)
if (startup_cnt == 0) {
// Send data when init phase is done
float pressure = NAVGO_BARO_SENS * (mcp355x_data + NAVGO_BARO_OFFSET);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
}
mcp355x_data_available = FALSE;
}
sw/airborne/boards/navstik/baro_board.c
+2 -2
View File
@@ -60,9 +60,9 @@ void baro_event(void)
if (baro_bmp085.data_available) {
float pressure = (float)baro_bmp085.pressure;
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
float temp = baro_bmp085.temperature / 10.0f;
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, &temp);
AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);
baro_bmp085.data_available = FALSE;
#ifdef BARO_LED
RunOnceEvery(10, LED_TOGGLE(BARO_LED));
sw/airborne/boards/umarim/baro_board.c
+1 -1
View File
@@ -78,7 +78,7 @@ void umarim_baro_event(void)
if (BARO_ABS_ADS.data_available) {
if (startup_cnt == 0) {
float pressure = UMARIM_BARO_SENS * Ads1114GetValue(BARO_ABS_ADS);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, &pressure);
AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
}
BARO_ABS_ADS.data_available = FALSE;
}
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:
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;
}
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_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;
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+19
View File
@@ -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;
}
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_CARE_FREE 6
#define GUIDANCE_H_MODE_FORWARD 7
#define GUIDANCE_H_MODE_MODULE 8
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 "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;
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_HOVER 4
#define GUIDANCE_V_MODE_NAV 5
#define GUIDANCE_V_MODE_MODULE 6
extern uint8_t guidance_v_mode;
sw/airborne/firmwares/rotorcraft/navigation.c
+20 -47
View File
@@ -48,9 +48,6 @@
#include "messages.h"
#include "mcu_periph/uart.h"
const uint8_t nb_waypoint = NB_WAYPOINT;
struct EnuCoor_i waypoints[NB_WAYPOINT];
struct EnuCoor_i navigation_target;
struct EnuCoor_i navigation_carrot;
@@ -136,30 +133,23 @@ static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
if (i >= nb_waypoint) { i = 0; }
pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
&i,
&(waypoints[i].x),
&(waypoints[i].y),
&(waypoints[i].z));
&(waypoints[i].enu_i.x),
&(waypoints[i].enu_i.y),
&(waypoints[i].enu_i.z));
}
#endif
void nav_init(void)
{
// convert to
const struct EnuCoor_f wp_tmp_float[NB_WAYPOINT] = WAYPOINTS_ENU;
// init int32 waypoints
uint8_t i = 0;
for (i = 0; i < nb_waypoint; i++) {
waypoints[i].x = POS_BFP_OF_REAL(wp_tmp_float[i].x);
waypoints[i].y = POS_BFP_OF_REAL(wp_tmp_float[i].y);
waypoints[i].z = POS_BFP_OF_REAL(wp_tmp_float[i].z);
}
waypoints_init();
nav_block = 0;
nav_stage = 0;
nav_altitude = POS_BFP_OF_REAL(SECURITY_HEIGHT);
nav_flight_altitude = nav_altitude;
flight_altitude = SECURITY_ALT;
VECT3_COPY(navigation_target, waypoints[WP_HOME]);
VECT3_COPY(navigation_carrot, waypoints[WP_HOME]);
VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
VECT3_COPY(navigation_carrot, waypoints[WP_HOME].enu_i);
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
vertical_mode = VERTICAL_MODE_ALT;
@@ -383,12 +373,15 @@ static inline void nav_set_altitude(void)
unit_t nav_reset_reference(void)
{
ins_reset_local_origin();
/* update local ENU coordinates of global waypoints */
nav_localize_global_waypoints();
return 0;
}
unit_t nav_reset_alt(void)
{
ins_reset_altitude_ref();
nav_localize_global_waypoints();
return 0;
}
@@ -414,28 +407,6 @@ void nav_periodic_task(void)
nav_run();
}
void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *new_lla_pos)
{
if (stateIsLocalCoordinateValid()) {
struct EnuCoor_i enu;
enu_of_lla_point_i(&enu, &state.ned_origin_i, new_lla_pos);
// convert ENU pos from cm to BFP with INT32_POS_FRAC
enu.x = POS_BFP_OF_REAL(enu.x) / 100;
enu.y = POS_BFP_OF_REAL(enu.y) / 100;
enu.z = POS_BFP_OF_REAL(enu.z) / 100;
nav_move_waypoint(wp_id, &enu);
}
}
void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i *new_pos)
{
if (wp_id < nb_waypoint) {
VECT3_COPY(waypoints[wp_id], (*new_pos));
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id, &(new_pos->x),
&(new_pos->y), &(new_pos->z));
}
}
void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp)
{
// MY_ASSERT(wp < nb_waypoint); FIXME
@@ -446,16 +417,18 @@ void navigation_update_wp_from_speed(uint8_t wp, struct Int16Vect3 speed_sp, int
struct Int32Vect3 delta_pos;
VECT3_SDIV(delta_pos, speed_sp, NAV_FREQ); /* fixme :make sure the division is really a >> */
INT32_VECT3_RSHIFT(delta_pos, delta_pos, (INT32_SPEED_FRAC - INT32_POS_FRAC));
waypoints[wp].x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC;
waypoints[wp].y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC;
waypoints[wp].z += delta_pos.z;
waypoints[wp].enu_i.x += (s_heading * delta_pos.x + c_heading * delta_pos.y) >> INT32_TRIG_FRAC;
waypoints[wp].enu_i.y += (c_heading * delta_pos.x - s_heading * delta_pos.y) >> INT32_TRIG_FRAC;
waypoints[wp].enu_i.z += delta_pos.z;
int32_t delta_heading = heading_rate_sp / NAV_FREQ;
delta_heading = delta_heading >> (INT32_SPEED_FRAC - INT32_POS_FRAC);
nav_heading += delta_heading;
INT32_COURSE_NORMALIZE(nav_heading);
RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp, &(waypoints[wp].x), &(waypoints[wp].y),
&(waypoints[wp].z)));
RunOnceEvery(10, DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp,
&(waypoints[wp].enu_i.x),
&(waypoints[wp].enu_i.y),
&(waypoints[wp].enu_i.z)));
}
bool_t nav_detect_ground(void)
@@ -474,10 +447,10 @@ bool_t nav_is_in_flight(void)
void nav_home(void)
{
horizontal_mode = HORIZONTAL_MODE_WAYPOINT;
VECT3_COPY(navigation_target, waypoints[WP_HOME]);
VECT3_COPY(navigation_target, waypoints[WP_HOME].enu_i);
vertical_mode = VERTICAL_MODE_ALT;
nav_altitude = waypoints[WP_HOME].z;
nav_altitude = waypoints[WP_HOME].enu_i.z;
nav_flight_altitude = nav_altitude;
dist2_to_wp = dist2_to_home;
@@ -499,7 +472,7 @@ float get_dist2_to_point(struct EnuCoor_i *p)
/** Returns squared horizontal distance to given waypoint */
float get_dist2_to_waypoint(uint8_t wp_id)
{
return get_dist2_to_point(&waypoints[wp_id]);
return get_dist2_to_point(&waypoints[wp_id].enu_i);
}
/** Computes squared distance to the HOME waypoint potentially sets
sw/airborne/firmwares/rotorcraft/navigation.h
+10 -21
View File
@@ -30,8 +30,8 @@
#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_geodetic_float.h"
#include "subsystems/navigation/waypoints.h"
#include "subsystems/navigation/common_flight_plan.h"
#define NAV_FREQ 16
@@ -39,9 +39,6 @@
extern struct EnuCoor_i navigation_target;
extern struct EnuCoor_i navigation_carrot;
extern struct EnuCoor_i waypoints[];
extern const uint8_t nb_waypoint;
extern void nav_init(void);
extern void nav_run(void);
@@ -84,8 +81,6 @@ extern void nav_home(void);
unit_t nav_reset_reference(void) __attribute__((unused));
unit_t nav_reset_alt(void) __attribute__((unused));
void nav_periodic_task(void);
void nav_move_waypoint_lla(uint8_t wp_id, struct LlaCoor_i *new_lla_pos);
void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i *new_pos);
bool_t nav_detect_ground(void);
bool_t nav_is_in_flight(void);
@@ -107,13 +102,7 @@ extern bool_t nav_set_heading_current(void);
#define NavSetGroundReferenceHere() ({ nav_reset_reference(); FALSE; })
#define NavSetAltitudeReferenceHere() ({ nav_reset_alt(); FALSE; })
#define NavSetWaypointHere(_wp) ({ VECT2_COPY(waypoints[_wp], *stateGetPositionEnu_i()); FALSE; })
#define NavCopyWaypoint(_wp1, _wp2) ({ VECT2_COPY(waypoints[_wp1], waypoints[_wp2]); FALSE; })
#define WaypointX(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].x)
#define WaypointY(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].y)
#define WaypointAlt(_wp) POS_FLOAT_OF_BFP(waypoints[_wp].z)
#define Height(_h) (_h)
#define NavSetWaypointHere(_wp) ({ nav_set_waypoint_here_2d(_wp); FALSE; })
/** Normalize a degree angle between 0 and 359 */
#define NormCourse(x) { \
@@ -124,7 +113,7 @@ extern bool_t nav_set_heading_current(void);
/*********** Navigation to waypoint *************************************/
#define NavGotoWaypoint(_wp) { \
horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
VECT3_COPY(navigation_target, waypoints[_wp]); \
VECT3_COPY(navigation_target, waypoints[_wp].enu_i); \
dist2_to_wp = get_dist2_to_waypoint(_wp); \
}
@@ -132,7 +121,7 @@ extern bool_t nav_set_heading_current(void);
extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius);
#define NavCircleWaypoint(_center, _radius) { \
horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
nav_circle(&waypoints[_center], POS_BFP_OF_REAL(_radius)); \
nav_circle(&waypoints[_center].enu_i, POS_BFP_OF_REAL(_radius)); \
}
#define NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI)
@@ -146,25 +135,25 @@ extern void nav_circle(struct EnuCoor_i *wp_center, int32_t radius);
extern void nav_route(struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end);
#define NavSegment(_start, _end) { \
horizontal_mode = HORIZONTAL_MODE_ROUTE; \
nav_route(&waypoints[_start], &waypoints[_end]); \
nav_route(&waypoints[_start].enu_i, &waypoints[_end].enu_i); \
}
/** Nav glide routine */
#define NavGlide(_last_wp, _wp) { \
int32_t start_alt = waypoints[_last_wp].z; \
int32_t diff_alt = waypoints[_wp].z - start_alt; \
int32_t start_alt = waypoints[_last_wp].enu_i.z; \
int32_t diff_alt = waypoints[_wp].enu_i.z - start_alt; \
int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \
NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \
}
/** Proximity tests on approaching a wp */
bool_t nav_approaching_from(struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time);
#define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp], NULL, time)
#define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp], &waypoints[from], time)
#define NavApproaching(wp, time) nav_approaching_from(&waypoints[wp].enu_i, NULL, time)
#define NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp].enu_i, &waypoints[from].enu_i, time)
/** Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp */
bool_t nav_check_wp_time(struct EnuCoor_i *wp, uint16_t stay_time);
#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp], time)
#define NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_i, time)
/** Set the climb control to auto-throttle with the specified pitch
pre-command */
sw/airborne/modules/air_data/air_data.c
+6 -6
View File
@@ -95,9 +95,9 @@ PRINT_CONFIG_MSG("USE_AIRSPEED_AIR_DATA automatically set to TRUE")
static uint8_t baro_health_counter;
static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure)
static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
{
air_data.pressure = *pressure;
air_data.pressure = pressure;
// calculate QNH from pressure and absolute alitude if that is available
if (air_data.calc_qnh_once && stateIsGlobalCoordinateValid()) {
@@ -116,9 +116,9 @@ static void pressure_abs_cb(uint8_t __attribute__((unused)) sender_id, const flo
baro_health_counter = 10;
}
static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure)
static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
{
air_data.differential = *pressure;
air_data.differential = pressure;
if (air_data.calc_airspeed) {
air_data.airspeed = tas_from_dynamic_pressure(air_data.differential);
#if USE_AIRSPEED_AIR_DATA
@@ -127,9 +127,9 @@ static void pressure_diff_cb(uint8_t __attribute__((unused)) sender_id, const fl
}
}
static void temperature_cb(uint8_t __attribute__((unused)) sender_id, const float *temp)
static void temperature_cb(uint8_t __attribute__((unused)) sender_id, float temp)
{
air_data.temperature = *temp;
air_data.temperature = temp;
if (air_data.calc_tas_factor && baro_health_counter > 0 && air_data.pressure > 0) {
air_data.tas_factor = get_tas_factor(air_data.pressure, air_data.temperature);
}
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 "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;
}
}
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
#define _VIDEO_H
#include "../../cv/image.h"
#include "modules/computer_vision/cv/image.h"
struct buffer_struct {
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

Some files were not shown because too many files have changed in this diff Show More