mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-10 15:09:25 +08:00
Merge pull request #2447 from enacuavlab/enac_student_projects-integration
Enac student projects integration
This commit is contained in:
Hector Garcia de Marina
GitHub
@@ -10,17 +10,22 @@
|
||||
<define name="LOW_NOISE_THRESHOLD" value="3500"/>
|
||||
<define name="LOW_NOISE_TIME" value="10"/>
|
||||
|
||||
<target name="ap" board="apogee_1.0_chibios">
|
||||
<target name="ap" board="chimera_1.0">
|
||||
</target>
|
||||
|
||||
<module name="radio_control" type="datalink">
|
||||
<!--define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/-->
|
||||
</module>
|
||||
|
||||
<module name="actuators" type="ostrich"/>
|
||||
<module name="actuators" type="ostrich">
|
||||
<configure name="ACTUATORS_OSTRICH_PORT" value="uart1"/>
|
||||
</module>
|
||||
|
||||
<module name="telemetry" type="xbee_api"/>
|
||||
<module name="imu" type="apogee"/>
|
||||
<module name="telemetry" type="transparent">
|
||||
<configure name="MODEM_BAUD" value="B57600"/>
|
||||
<!--configure name="MODEM_BAUD" value="UART3"/-->
|
||||
</module>
|
||||
<module name="imu" type="chimera"/>
|
||||
|
||||
<module name="gps" type="datalink">
|
||||
<configure name="USE_MAGNETOMETER" value="FALSE"/>
|
||||
@@ -67,12 +72,12 @@
|
||||
|
||||
<section name="IMU" prefix="IMU_">
|
||||
|
||||
<define name="GYRO_P_SIGN" value="-1"/>
|
||||
<define name="GYRO_Q_SIGN" value="1"/>
|
||||
<define name="GYRO_R_SIGN" value="-1"/>
|
||||
<define name="GYRO_P_SIGN" value="1"/>
|
||||
<define name="GYRO_Q_SIGN" value="-1"/>
|
||||
<define name="GYRO_R_SIGN" value="-1"/> <!-- ugly hack for dead accel -->
|
||||
|
||||
<define name="ACCEL_X_SIGN" value="-1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="1"/>
|
||||
<define name="ACCEL_X_SIGN" value="1"/>
|
||||
<define name="ACCEL_Y_SIGN" value="-1"/>
|
||||
<define name="ACCEL_Z_SIGN" value="-1"/>
|
||||
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
@@ -99,6 +104,10 @@
|
||||
<define name="TURN_IGAIN" value="0."/>
|
||||
</section>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="DEFAULT_CIRCLE_RADIUS" value="1.5"/>
|
||||
</section>
|
||||
|
||||
<section name="SIMULATOR" prefix="NPS_">
|
||||
<define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="HOOPERFLY/hooperfly_teensyfly_quad" type="string"/>
|
||||
|
||||
@@ -0,0 +1,97 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="260" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
|
||||
<header>
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0" y="0"/>
|
||||
<waypoint name="STDBY" x="49.5" y="100.1"/>
|
||||
<waypoint name="1" x="10.1" y="189.9"/>
|
||||
<waypoint name="2" x="132.3" y="139.1"/>
|
||||
<waypoint name="MOB" x="137.0" y="-11.6"/>
|
||||
<waypoint alt="215.0" name="AF" x="177.4" y="45.1"/>
|
||||
<waypoint alt="185.0" name="TD" x="-22.6" y="45.1"/>
|
||||
<waypoint name="_BASELEG" x="168.8" y="-13.8"/>
|
||||
<waypoint name="CLIMB" x="-114.5" y="162.3"/>
|
||||
<waypoint name="ANCHOR_1" x="-15" y="37.5"/>
|
||||
<waypoint name="ANCHOR_2" x="-20" y="40."/>
|
||||
<waypoint name="ANCHOR_3" x="-20" y="35."/>
|
||||
</waypoints>
|
||||
<modules>
|
||||
<module name="shift_tracking"/>
|
||||
</modules>
|
||||
<exceptions/>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call_once fun="dw1000_use_ekf = false"/>
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 10)"/>
|
||||
<call_once fun="NavSetGroundReferenceHere()"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<set value="1" var="autopilot.kill_throttle"/>
|
||||
<attitude roll="0" throttle="0" vmode="throttle"/>
|
||||
</block>
|
||||
<block group="home" key="t" name="Takeoff" strip_button="Takeoff (wp CLIMB)" strip_icon="takeoff.png" group="home">
|
||||
<exception cond="GetPosAlt() > GetAltRef()+25" deroute="Standby"/>
|
||||
<set value="0" var="autopilot.kill_throttle"/>
|
||||
<set value="0" var="autopilot.flight_time"/>
|
||||
<go from="HOME" pitch="15" throttle="1.0" vmode="throttle" wp="CLIMB"/>
|
||||
</block>
|
||||
<block group="home" key="Ctrl+a" name="Standby" strip_button="Standby" strip_icon="home.png" group="home">
|
||||
<circle radius="nav_radius" wp="STDBY"/>
|
||||
</block>
|
||||
<block name="Oval 1-2" strip_button="Oval (wp 1-2)" strip_icon="oval.png" group="base_pattern">
|
||||
<oval p1="1" p2="2" radius="nav_radius"/>
|
||||
</block>
|
||||
<block name="Start Shift Tracking">
|
||||
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
|
||||
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
|
||||
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
|
||||
</block>
|
||||
<block name="Run Shift Tracking" pre_call="shift_tracking_run(&nav_shift)">
|
||||
<call_once fun="shift_tracking_reset()"/>
|
||||
<call_once fun="dw1000_use_ekf = true"/>
|
||||
<go approaching_time="0" from="AF" hmode="route" wp="TD" alt="WaypointAlt(WP_AF)"/>
|
||||
<call_once fun="dw1000_use_ekf = false"/>
|
||||
<deroute block="Start Shift Tracking"/>
|
||||
</block>
|
||||
<block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
|
||||
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block group="land" name="Land Left AF-TD" strip_button="Land left (wp AF-TD)" strip_icon="land-left.png">
|
||||
<set value="-DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
|
||||
<deroute block="land"/>
|
||||
</block>
|
||||
<block name="land">
|
||||
<call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
|
||||
<circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
|
||||
<circle radius="nav_radius" until="And(NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10), 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG)))" wp="_BASELEG"/>
|
||||
</block>
|
||||
<block name="final">
|
||||
<exception cond="GetAltRef() + 10 > GetPosAlt()" deroute="flare"/>
|
||||
<go from="AF" hmode="route" vmode="glide" wp="TD"/>
|
||||
</block>
|
||||
<block name="flare">
|
||||
<go approaching_time="0" from="AF" hmode="route" throttle="0.0" vmode="throttle" wp="TD"/>
|
||||
<attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Set Anchor 1">
|
||||
<call_once fun="NavSetWaypointHere(WP_ANCHOR_1)"/>
|
||||
<deroute block="Holding point"/>
|
||||
</block>
|
||||
<block name="Set Anchor 2">
|
||||
<call_once fun="NavSetWaypointHere(WP_ANCHOR_2)"/>
|
||||
<deroute block="Holding point"/>
|
||||
</block>
|
||||
<block name="Set Anchor 3">
|
||||
<call_once fun="NavSetWaypointHere(WP_ANCHOR_3)"/>
|
||||
<deroute block="Holding point"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -0,0 +1,72 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="148" ground_alt="146" lat0="43.5640917" lon0="1.4829201" max_dist_from_home="20" name="Rover Optitrack (ENAC)" security_height="0.3">
|
||||
<header>
|
||||
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint name="HOME" x="0.0" y="0.0"/>
|
||||
<waypoint name="GOAL" x="2.0" y="2.0"/>
|
||||
<waypoint name="STDBY" x="-0.7" y="-0.8"/>
|
||||
<waypoint name="TD" x="0.8" y="-1.7"/>
|
||||
<waypoint name="P1" x="-1" y="-4"/>
|
||||
<waypoint name="P2" x="-1" y="4"/>
|
||||
<waypoint name="_S1" x="3" y="4"/>
|
||||
<waypoint name="_S2" x="3" y="-4"/>
|
||||
<waypoint name="_S3" x="-3" y="-4"/>
|
||||
<waypoint name="_S4" x="-3" y="4"/>
|
||||
<waypoint name="_N1" x="4.5" y="5.2"/>
|
||||
<waypoint name="_N2" x="4.5" y="-5.2"/>
|
||||
<waypoint name="_N3" x="-4.5" y="-5.2"/>
|
||||
<waypoint name="_N4" x="-4.5" y="5.2"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector name="Net" color="red">
|
||||
<corner name="_N1"/>
|
||||
<corner name="_N2"/>
|
||||
<corner name="_N3"/>
|
||||
<corner name="_N4"/>
|
||||
</sector>
|
||||
<sector name="Survey" color="green" type="dynamic">
|
||||
<corner name="_S1"/>
|
||||
<corner name="_S2"/>
|
||||
<corner name="_S3"/>
|
||||
<corner name="_S4"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<variables>
|
||||
<variable var="turn_rate" init="10." min="0." max="50." step="1.0"/>
|
||||
</variables>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
<block name="line_P1_P2">
|
||||
<go wp="P1" approaching_time="0"/>
|
||||
<while cond="TRUE">
|
||||
<go from="P1" hmode="route" wp="P2" approaching_time="0"/>
|
||||
<stay wp="P2" until="stage_time>10"/>
|
||||
<go from="P2" hmode="route" wp="P1" approaching_time="0"/>
|
||||
</while>
|
||||
</block>
|
||||
<block name="circle">
|
||||
<circle radius="nav.radius" wp="STDBY"/>
|
||||
</block>
|
||||
<block name="line_P1_P2_turn">
|
||||
<go wp="P1" approaching_time="0"/>
|
||||
<while cond="TRUE">
|
||||
<go from="P1" hmode="route" wp="P2" approaching_time="0" pre_call="nav_set_heading_deg(turn_rate*block_time)"/>
|
||||
<stay wp="P2" until="stage_time>10" pre_call="nav_set_heading_deg(turn_rate*block_time)"/>
|
||||
<go from="P2" hmode="route" wp="P1" approaching_time="0" pre_call="nav_set_heading_deg(turn_rate*block_time)"/>
|
||||
</while>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -8,9 +8,13 @@
|
||||
Decawave DW1000 modules (http://www.decawave.com/products/dwm1000-module) are Ultra-Wide-Band devices that can be used for communication and ranging.
|
||||
Especially, using 3 modules as anchors can provide data for a localization system based on trilateration.
|
||||
The DW1000 is using a SPI connection, but an arduino-compatible board can be used with the library https://github.com/thotro/arduino-dw1000 to hyde the low level drivers and provide direct ranging informations.
|
||||
|
||||
See https://hal-enac.archives-ouvertes.fr/hal-01936955 for more information on the EKF filtering.
|
||||
</description>
|
||||
<configure name="DW1000_ARDUINO_UART" value="UARTX" description="UART on which arduino and its DW1000 module is connected"/>
|
||||
<configure name="DW1000_ARDUINO_BAUD" value="B115200" description="UART Baudrate, default to 115200"/>
|
||||
<configure name="DW1000_USE_AS_LOCAL_POS" value="FALSE|TRUE" description="use as a local positioning system (default: TRUE)"/>
|
||||
<configure name="DW1000_USE_AS_GPS" value="FALSE|TRUE" description="use as a fake GPS positioning system (default: FALSE)"/>
|
||||
<section name="DW1000" prefix="DW1000_">
|
||||
<define name="ANCHORS_IDS" value="1, 2, 3" type="int[]" description="Comma separated list of anchors ID"/>
|
||||
<define name="ANCHORS_POS_X" value="0., 0., 5." type="float[]" description="Comma separated list of anchors ID over X axis"/>
|
||||
@@ -20,8 +24,24 @@
|
||||
<define name="SCALE" value="1., 1., 1." type="float[]" description="Position scale factor other X, Y and Z axis"/>
|
||||
<define name="INITIAL_HEADING" value="0." description="Initial heading correction between anchors frame and global frame"/>
|
||||
<define name="NB_ANCHORS" value="3" description="Set number of anchors, only 3 are required/supported at the moment"/>
|
||||
<define name="USE_EKF" value="FALSE|TRUE" description="Enable EKF filtering, required to estimate speed"/>
|
||||
<define name="EKF_Q" value="1.0" description="EKF process noise"/>
|
||||
<define name="EKF_R_DIST" value="0.1" description="EKF noise on distance measurements"/>
|
||||
<define name="EKF_R_SPEED" value="0.1" description="EKF noise on speed measurements (if available)"/>
|
||||
<define name="NOISE_X|Y|Z" value="0.1" description="Noise level reported by the POSITION_ESTIMATE message when USE_AS_LOCAL_POS is activated"/>
|
||||
<define name="VEL_NOISE_X|Y|Z" value="0.1" description="Noise level reported by the VELOCITY_ESTIMATE message when USE_AS_LOCAL_POS is activated"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings name="dw1000">
|
||||
<dl_setting max="1" min="0" step="1" module="decawave/dw1000_arduino" var="dw1000_use_ekf" shortname="use_ekf" values="FALSE|TRUE"/>
|
||||
<dl_setting max="5.0" min="0.01" step="0.01" module="decawave/dw1000_arduino" var="dw1000_ekf_q" shortname="q_noise" handler="update_ekf_q"/>
|
||||
<dl_setting max="1.0" min="0.01" step="0.01" module="decawave/dw1000_arduino" var="dw1000_ekf_r_dist" shortname="r_dist" handler="update_ekf_r_dist"/>
|
||||
<dl_setting max="1.0" min="0.01" step="0.01" module="decawave/dw1000_arduino" var="dw1000_ekf_r_speed" shortname="r_speed" handler="update_ekf_r_speed"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="dw1000_arduino.h"/>
|
||||
</header>
|
||||
@@ -33,8 +53,32 @@
|
||||
<makefile>
|
||||
<configure name="DW1000_ARDUINO_UART" case="upper|lower"/>
|
||||
<configure name="DW1000_ARDUINO_BAUD" default="B115200"/>
|
||||
<configure name="DW1000_USE_AS_LOCAL_POS" default="TRUE"/>
|
||||
<configure name="DW1000_USE_AS_GPS" default="FALSE"/>
|
||||
<file name="dw1000_arduino.c"/>
|
||||
<file name="trilateration.c"/>
|
||||
<file name="ekf_range.c"/>
|
||||
<define name="DW1000_USE_AS_LOCAL_POS" value="$(DW1000_USE_AS_LOCAL_POS)"/>
|
||||
<define name="DW1000_USE_AS_GPS" value="$(DW1000_USE_AS_GPS)"/>
|
||||
<raw>
|
||||
ifeq (,$(findstring $(DW1000_USE_AS_GPS),0 FALSE))
|
||||
ifdef SECONDARY_GPS
|
||||
ifneq (,$(findstring $(SECONDARY_GPS), dw1000))
|
||||
# this is the secondary GPS
|
||||
$(TARGET).CFLAGS += -DGPS_SECONDARY_TYPE_H=\"modules/decawave/dw1000_arduino.h\"
|
||||
$(TARGET).CFLAGS += -DSECONDARY_GPS=GPS_DW1000
|
||||
else
|
||||
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/decawave/dw1000_arduino.h\"
|
||||
$(TARGET).CFLAGS += -DPRIMARY_GPS=GPS_DW1000
|
||||
endif
|
||||
else
|
||||
# plain old single GPS usage
|
||||
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"modules/decawave/dw1000_arduino.h\"
|
||||
endif
|
||||
endif
|
||||
</raw>
|
||||
</makefile>
|
||||
<makefile target="ap">
|
||||
<define name="USE_$(DW1000_ARDUINO_UART_UPPER)"/>
|
||||
<define name="DW1000_ARDUINO_DEV" value="$(DW1000_ARDUINO_UART_LOWER)"/>
|
||||
<define name="$(DW1000_ARDUINO_UART_UPPER)_BAUD" value="$(DW1000_ARDUINO_BAUD)"/>
|
||||
|
||||
@@ -9,6 +9,7 @@
|
||||
</description>
|
||||
<configure name="JEVOIS_UART" value="UARTX" description="UART on which Jevois camera is connected"/>
|
||||
<configure name="JEVOIS_BAUD" value="B115200" description="UART Baudrate, default to 115200"/>
|
||||
<define name="JEVOIS_SEND_MSG" value="FALSE|TRUE" description="Send synchronized jevois message over telemetry link, use periodic report function otherwise"/>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
@@ -22,6 +23,7 @@
|
||||
<file name="jevois.h"/>
|
||||
</header>
|
||||
<init fun="jevois_init()"/>
|
||||
<periodic fun="jevois_report()" freq="5" autorun="FALSE"/>
|
||||
<event fun="jevois_event()"/>
|
||||
<makefile>
|
||||
<configure name="JEVOIS_UART" case="upper|lower"/>
|
||||
|
||||
@@ -0,0 +1,43 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="shift_tracking" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Pilot nav shift variable to track an offset in navigation
|
||||
|
||||
Typical use is to Control a fixed-wing to use both GPS and
|
||||
local position (from Decawave modules for instance) to land it
|
||||
with precision or in a net.
|
||||
|
||||
Data are coming from POSITION_ESTIMATE ABI message.
|
||||
|
||||
See https://hal-enac.archives-ouvertes.fr/hal-01936955 for more information on UAV landing in a net.
|
||||
</description>
|
||||
<section name="SHIFT_TRACKING" prefix="SHIFT_TRACKING_">
|
||||
<define name="ID" value="ABI_BROADCAST" description="ABI binding ID"/>
|
||||
<define name="DIR" value="1.0, 0., 0." type="float[]" description="direction to track expressed in the local pos frame"/>
|
||||
<define name="KP" value="1.5" description="proportional gain"/>
|
||||
<define name="KI" value="0.5" description="integral gain"/>
|
||||
<define name="KD" value="1.0" description="derivative gain"/>
|
||||
<define name="MAXSHIFT" value="30." description="maximum offset control"/>
|
||||
<define name="MAXSUM" value="30." description="maximum integral part"/>
|
||||
</section>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="shift tracking">
|
||||
<dl_setting MAX="5." MIN="0." STEP="0.05" VAR="shift_tracking.kp" module="modules/ctrl/shift_tracking" handler="SetKp"/>
|
||||
<dl_setting MAX="5." MIN="0." STEP="0.05" VAR="shift_tracking.ki" module="modules/ctrl/shift_tracking" handler="SetKi"/>
|
||||
<dl_setting MAX="5." MIN="0." STEP="0.05" VAR="shift_tracking.kd" module="modules/ctrl/shift_tracking" handler="SetKd"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
<header>
|
||||
<file name="shift_tracking.h"/>
|
||||
</header>
|
||||
<init fun="shift_tracking_init()"/>
|
||||
<makefile>
|
||||
<file name="shift_tracking.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
@@ -44,5 +44,9 @@
|
||||
<file name="rotorcraft_datalink.c" dir="$(SRC_FIRMWARE)"/>
|
||||
<file name="rotorcraft_telemetry.c" dir="$(SRC_FIRMWARE)"/>
|
||||
</makefile>
|
||||
<makefile target="ap" firmware="rover">
|
||||
<file name="rover_datalink.c" dir="$(SRC_FIRMWARE)"/>
|
||||
<file name="rover_telemetry.c" dir="$(SRC_FIRMWARE)"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -53,5 +53,9 @@
|
||||
<file name="rotorcraft_datalink.c" dir="$(SRC_FIRMWARE)"/>
|
||||
<file name="rotorcraft_telemetry.c" dir="$(SRC_FIRMWARE)"/>
|
||||
</makefile>
|
||||
<makefile target="ap" firmware="rover">
|
||||
<file name="rover_datalink.c" dir="$(SRC_FIRMWARE)"/>
|
||||
<file name="rover_telemetry.c" dir="$(SRC_FIRMWARE)"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -45,5 +45,9 @@
|
||||
<file name="rotorcraft_datalink.c" dir="$(SRC_FIRMWARE)"/>
|
||||
<file name="rotorcraft_telemetry.c" dir="$(SRC_FIRMWARE)"/>
|
||||
</makefile>
|
||||
<makefile target="ap" firmware="rover">
|
||||
<file name="rover_datalink.c" dir="$(SRC_FIRMWARE)"/>
|
||||
<file name="rover_telemetry.c" dir="$(SRC_FIRMWARE)"/>
|
||||
</makefile>
|
||||
</module>
|
||||
|
||||
|
||||
@@ -3,7 +3,7 @@
|
||||
<settings>
|
||||
<dl_settings>
|
||||
<dl_settings NAME="sonar">
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="ins_impl.update_on_agl" shortname="use_sonar" values="FALSE|TRUE" module="subsystems/ins/vf_extended_float"/>
|
||||
<dl_setting MAX="1" MIN="0" STEP="1" VAR="ins_int.update_on_agl" shortname="use_sonar" values="FALSE|TRUE" module="subsystems/ins/vf_extended_float"/>
|
||||
</dl_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
|
||||
@@ -26,6 +26,20 @@
|
||||
<message name="UART_ERRORS" period="3.1"/>
|
||||
</mode>
|
||||
|
||||
<mode name="low" key_press="l">
|
||||
<message name="AUTOPILOT_VERSION" period="11.1"/>
|
||||
<message name="ROVER_STATUS" period="1.2"/>
|
||||
<message name="ROTORCRAFT_FP" period="1.0"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
<message name="DL_VALUE" period="1.1"/>
|
||||
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
|
||||
<message name="WP_MOVED" period="1.3"/>
|
||||
<message name="INS_REF" period="5.1"/>
|
||||
<message name="ENERGY" period="2.5"/>
|
||||
<message name="DATALINK_REPORT" period="5.1"/>
|
||||
<message name="STATE_FILTER_STATUS" period="3.2"/>
|
||||
</mode>
|
||||
|
||||
<mode name="rc">
|
||||
<message name="DL_VALUE" period="0.5"/>
|
||||
<message name="ALIVE" period="2.1"/>
|
||||
|
||||
@@ -0,0 +1,338 @@
|
||||
/*
|
||||
* Copyright (C) 2018 Gautier Hattenberger
|
||||
*
|
||||
* 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 filters/pid.h
|
||||
* @brief Several forms of PID controllers
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef PID_H
|
||||
#define PID_H
|
||||
|
||||
/** Simple PID structure
|
||||
* floating point
|
||||
*
|
||||
* u_k = Kp * e_k + Kd * (e_k - e_k-1) / dt + Ki * (sum (e_k * dt))
|
||||
*
|
||||
* with:
|
||||
* u = outputs
|
||||
* e = inputs
|
||||
* Kp = proportional gain
|
||||
* Kd = derivative gain
|
||||
* Ki = integral gain
|
||||
* dt = time since last input
|
||||
*/
|
||||
struct PID_f {
|
||||
float u; ///< output
|
||||
float e[2]; ///< input
|
||||
float sum; ///< integral of input
|
||||
float g[3]; ///< controller gains (Kp, Kd, Ki)
|
||||
float max_sum; ///< windup protection, max of Ki * sum(e_k * dt)
|
||||
};
|
||||
|
||||
static inline void init_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki, float max_sum)
|
||||
{
|
||||
*pid = (struct PID_f) {
|
||||
0.f,
|
||||
{ 0.f , 0.f },
|
||||
0.f,
|
||||
{ Kp, Kd, Ki },
|
||||
max_sum
|
||||
};
|
||||
}
|
||||
|
||||
/** Update PID with a new value and return new command.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @param value new input value of the PID
|
||||
* @param dt time since last input (in seconds)
|
||||
* @return new output command
|
||||
*/
|
||||
static inline float update_pid_f(struct PID_f *pid, float value, float dt)
|
||||
{
|
||||
pid->e[1] = pid->e[0];
|
||||
pid->e[0] = value;
|
||||
float integral = pid->g[2] * (pid->sum + value);
|
||||
if (integral > pid->max_sum) {
|
||||
integral = pid->max_sum;
|
||||
} else if (integral < -pid->max_sum) {
|
||||
integral = -pid->max_sum;
|
||||
} else {
|
||||
pid->sum += value;
|
||||
}
|
||||
pid->u = pid->g[0] * pid->e[0] + pid->g[1] * (pid->e[0] - pid->e[1]) / dt + integral;
|
||||
return pid->u;
|
||||
}
|
||||
|
||||
/** Get current value of the PID command.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @return current value of PID command
|
||||
*/
|
||||
static inline float get_pid_f(struct PID_f *pid)
|
||||
{
|
||||
return pid->u;
|
||||
}
|
||||
|
||||
/** Reset PID struture, gains left unchanged.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
*/
|
||||
static inline void reset_pid_f(struct PID_f *pid)
|
||||
{
|
||||
pid->u = 0.f;
|
||||
pid->e[0] = 0.f;
|
||||
pid->e[1] = 0.f;
|
||||
pid->sum = 0.f;
|
||||
}
|
||||
|
||||
/** Set gains of the PID struct.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @param Kp proportional gain
|
||||
* @param Kd derivative gain
|
||||
* @param Ki integral gain
|
||||
*/
|
||||
static inline void set_gains_pid_f(struct PID_f *pid, float Kp, float Kd, float Ki)
|
||||
{
|
||||
pid->g[0] = Kp;
|
||||
pid->g[1] = Kd;
|
||||
pid->g[2] = Ki;
|
||||
}
|
||||
|
||||
/** Set integral part, can be used to reset.
|
||||
* The new sum of errors is calculated from current gains and bounds.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @param value integral part of the PID control, 0. will reset it
|
||||
*/
|
||||
static inline void set_integral_pid_f(struct PID_f *pid, float value)
|
||||
{
|
||||
float integral = value;
|
||||
if (integral < -pid->max_sum) {
|
||||
integral = -pid->max_sum;
|
||||
} else if (integral > pid->max_sum) {
|
||||
integral = pid->max_sum;
|
||||
}
|
||||
if (fabsf(pid->g[2]) < 1e-6) {
|
||||
pid->sum = 0.f; // integral gain is too low, prevent division by zero, just reset sum
|
||||
} else {
|
||||
pid->sum = integral / pid->g[2];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
/** Distcrete time PID structure.
|
||||
* floating point, fixed frequency.
|
||||
*
|
||||
* u_k = u_k-1 + a * e_k + b * e_k-1 + c * e_k-2
|
||||
*
|
||||
* with:
|
||||
* u = outputs
|
||||
* e = inputs
|
||||
* a = Kp + Ki Ts/2 + Kd/Ts
|
||||
* b = -Kp + Ki Ts/2 - 2 Kd/Ts
|
||||
* c = Kd/Ts
|
||||
* Kp = proportional gain
|
||||
* Kd = derivative gain
|
||||
* Ki = integral gain
|
||||
* Ts = sampling frequency
|
||||
*
|
||||
*/
|
||||
struct PID_df {
|
||||
float u[2]; ///< output
|
||||
float e[3]; ///< input
|
||||
float g[3]; ///< controller gains
|
||||
};
|
||||
|
||||
/** Init PID struct.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @param Kp proportional gain
|
||||
* @param Kd derivative gain
|
||||
* @param Ki integral gain
|
||||
* @param Ts sampling time
|
||||
*/
|
||||
static inline void init_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
|
||||
{
|
||||
*pid = (struct PID_df) {
|
||||
{ 0.f, 0.f },
|
||||
{ 0.f ,0.f , 0.f },
|
||||
{ Kp + Ki * Ts / 2.f + Kd / Ts,
|
||||
-Kp + Ki * Ts / 2.f - 2.f * Kd / Ts,
|
||||
Kd / Ts }
|
||||
};
|
||||
}
|
||||
|
||||
/** Update PID with a new value and return new command.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @param value new input value of the PID
|
||||
* @return new output command
|
||||
*/
|
||||
static inline float update_pid_df(struct PID_df *pid, float value)
|
||||
{
|
||||
pid->e[2] = pid->e[1];
|
||||
pid->e[1] = pid->e[0];
|
||||
pid->e[0] = value;
|
||||
pid->u[1] = pid->u[0];
|
||||
pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * pid->e[2];
|
||||
return pid->u[0];
|
||||
}
|
||||
|
||||
/** Get current value of the PID command.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @return current value of PID command
|
||||
*/
|
||||
static inline float get_pid_df(struct PID_df *pid)
|
||||
{
|
||||
return pid->u[0];
|
||||
}
|
||||
|
||||
/** Reset PID struture, gains left unchanged.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
*/
|
||||
static inline void reset_pid_df(struct PID_df *pid)
|
||||
{
|
||||
pid->u[0] = 0.f;
|
||||
pid->u[1] = 0.f;
|
||||
pid->e[0] = 0.f;
|
||||
pid->e[1] = 0.f;
|
||||
pid->e[2] = 0.f;
|
||||
}
|
||||
|
||||
/** Set gains of the PID struct.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @param Kp proportional gain
|
||||
* @param Kd derivative gain
|
||||
* @param Ki integral gain
|
||||
* @param Ts sampling time
|
||||
*/
|
||||
static inline void set_gains_pid_df(struct PID_df *pid, float Kp, float Kd, float Ki, float Ts)
|
||||
{
|
||||
pid->g[0] = Kp + Ki * Ts / 2.f + Kd / Ts;
|
||||
pid->g[1] = -Kp + Ki * Ts / 2.f - 2.f * Kd / Ts;
|
||||
pid->g[2] = Kd / Ts;
|
||||
}
|
||||
|
||||
/** Distcrete time PI-D structure.
|
||||
* derivative term is directly provided as input
|
||||
* as it may be available directly from a sensor
|
||||
* or estimated separately.
|
||||
* floating point, fixed frequency.
|
||||
*
|
||||
* u_k = u_k-1 + a * e_k + b * e_k-1 + Kd * d_k
|
||||
*
|
||||
* with:
|
||||
* u = outputs
|
||||
* e = inputs
|
||||
* d = derivative of input
|
||||
* a = Kp + Ki Ts/2
|
||||
* b = -Kp + Ki Ts/2
|
||||
* Kp = proportional gain
|
||||
* Kd = derivative gain
|
||||
* Ki = integral gain
|
||||
* Ts = sampling frequency
|
||||
*
|
||||
*/
|
||||
struct PI_D_df {
|
||||
float u[2]; ///< output
|
||||
float e[2]; ///< input
|
||||
float g[3]; ///< controller gains
|
||||
};
|
||||
|
||||
/** Init PI-D struct.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @param Kp proportional gain
|
||||
* @param Kd derivative gain
|
||||
* @param Ki integral gain
|
||||
* @param Ts sampling time
|
||||
*/
|
||||
static inline void init_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
|
||||
{
|
||||
*pid = (struct PI_D_df) {
|
||||
{ 0.f, 0.f },
|
||||
{ 0.f ,0.f },
|
||||
{ Kp + Ki * Ts / 2.f,
|
||||
-Kp + Ki * Ts / 2.f,
|
||||
Kd }
|
||||
};
|
||||
}
|
||||
|
||||
/** Update PI-D with a new value and return new command.
|
||||
*
|
||||
* @param pid pointer to PI-D structure
|
||||
* @param value new input value of the PI-D
|
||||
* @param deriv new input derivative
|
||||
* @return new output command
|
||||
*/
|
||||
static inline float update_pi_d_df(struct PI_D_df *pid, float value, float deriv)
|
||||
{
|
||||
pid->e[1] = pid->e[0];
|
||||
pid->e[0] = value;
|
||||
pid->u[1] = pid->u[0];
|
||||
pid->u[0] = pid->u[1] + pid->g[0] * pid->e[0] + pid->g[1] * pid->e[1] + pid->g[2] * deriv;
|
||||
return pid->u[0];
|
||||
}
|
||||
|
||||
/** Get current value of the PI-D command.
|
||||
*
|
||||
* @param pid pointer to PI-D structure
|
||||
* @return current value of PI-D command
|
||||
*/
|
||||
static inline float get_pi_d_df(struct PI_D_df *pid)
|
||||
{
|
||||
return pid->u[0];
|
||||
}
|
||||
|
||||
/** Reset PI-D struture, gains left unchanged.
|
||||
*
|
||||
* @param pid pointer to PI-D structure
|
||||
*/
|
||||
static inline void reset_pi_d_df(struct PI_D_df *pid)
|
||||
{
|
||||
pid->u[0] = 0.f;
|
||||
pid->u[1] = 0.f;
|
||||
pid->e[0] = 0.f;
|
||||
pid->e[1] = 0.f;
|
||||
}
|
||||
|
||||
/** Set gains PI-D struct.
|
||||
*
|
||||
* @param pid pointer to PID structure
|
||||
* @param Kp proportional gain
|
||||
* @param Kd derivative gain
|
||||
* @param Ki integral gain
|
||||
* @param Ts sampling time
|
||||
*/
|
||||
static inline void set_gains_pi_d_df(struct PI_D_df *pid, float Kp, float Kd, float Ki, float Ts)
|
||||
{
|
||||
pid->g[0] = Kp + Ki * Ts / 2.f;
|
||||
pid->g[1] = -Kp + Ki * Ts / 2.f;
|
||||
pid->g[2] = Kd;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,166 @@
|
||||
/*
|
||||
* Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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/shift_tracking.h"
|
||||
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* pilot the nav shift variable to track an offset trajectory
|
||||
* based on the POSITION_ESTIMATE ABI message
|
||||
*/
|
||||
|
||||
#include "modules/ctrl/shift_tracking.h"
|
||||
|
||||
#include "firmwares/fixedwing/nav.h"
|
||||
#include "autopilot.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "generated/airframe.h"
|
||||
|
||||
#include "filters/pid.h"
|
||||
|
||||
// ABI message binding ID
|
||||
#ifndef SHIFT_TRACKING_ID
|
||||
#define SHIFT_TRACKING_ID ABI_BROADCAST
|
||||
#endif
|
||||
|
||||
// Send debug message
|
||||
#ifndef SHIFT_TRACKING_DEBUG
|
||||
#define SHIFT_TRACKING_DEBUG FALSE
|
||||
#endif
|
||||
|
||||
#if SHIFT_TRACKING_DEBUG
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#include "pprzlink/messages.h"
|
||||
#include "mcu_periph/uart.h"
|
||||
#endif
|
||||
|
||||
#ifndef SHIFT_TRACKING_DIR
|
||||
#define SHIFT_TRACKING_DIR { -1.0f, 0.f, 0.f }
|
||||
#endif
|
||||
|
||||
#ifndef SHIFT_TRACKING_KP
|
||||
#define SHIFT_TRACKING_KP 1.5f
|
||||
#endif
|
||||
|
||||
#ifndef SHIFT_TRACKING_KI
|
||||
#define SHIFT_TRACKING_KI 0.5f
|
||||
#endif
|
||||
|
||||
#ifndef SHIFT_TRACKING_KD
|
||||
#define SHIFT_TRACKING_KD 1.f
|
||||
#endif
|
||||
|
||||
#ifndef SHIFT_TRACKING_MAXSHIFT
|
||||
#define SHIFT_TRACKING_MAXSHIFT 30.f
|
||||
#endif
|
||||
|
||||
struct shift_tracking_t shift_tracking;
|
||||
|
||||
// base time on NAV freq
|
||||
static const float nav_dt = 1.f / NAVIGATION_FREQUENCY;
|
||||
|
||||
// internal structure
|
||||
struct shift_tracking_private {
|
||||
struct FloatVect3 pos; ///< last position report
|
||||
struct FloatVect3 dir; ///< tracking direction
|
||||
struct PID_f pid; ///< PID controller
|
||||
float *shift; ///< keep track of the shift variable to change
|
||||
abi_event pos_ev;
|
||||
};
|
||||
|
||||
static struct shift_tracking_private stp;
|
||||
|
||||
static const float dir[] = SHIFT_TRACKING_DIR;
|
||||
|
||||
// callback on follow target message
|
||||
static void get_pos(uint8_t sender_id __attribute__((unused)),
|
||||
uint32_t id __attribute__((unused)),
|
||||
float x,
|
||||
float y,
|
||||
float z,
|
||||
float noise_x __attribute__((unused)),
|
||||
float noise_y __attribute__((unused)),
|
||||
float noise_z __attribute__((unused)))
|
||||
{
|
||||
stp.pos.x = x;
|
||||
stp.pos.y = y;
|
||||
stp.pos.z = z;
|
||||
}
|
||||
|
||||
void shift_tracking_init(void)
|
||||
{
|
||||
shift_tracking.kp = SHIFT_TRACKING_KP;
|
||||
shift_tracking.ki = SHIFT_TRACKING_KI;
|
||||
shift_tracking.kd = SHIFT_TRACKING_KD;
|
||||
shift_tracking.shift = 0.f;
|
||||
|
||||
FLOAT_VECT3_ZERO(stp.pos);
|
||||
stp.dir.x = dir[0];
|
||||
stp.dir.y = dir[1];
|
||||
stp.dir.z = dir[2];
|
||||
float_vect3_normalize(&stp.dir); // normalize direction
|
||||
init_pid_f(&stp.pid, shift_tracking.kp, shift_tracking.kd, shift_tracking.ki, 30.f);
|
||||
stp.shift = NULL;
|
||||
|
||||
// Bind to position message
|
||||
AbiBindMsgPOSITION_ESTIMATE(SHIFT_TRACKING_ID, &stp.pos_ev, get_pos);
|
||||
}
|
||||
|
||||
void shift_tracking_reset(void)
|
||||
{
|
||||
reset_pid_f(&stp.pid);
|
||||
shift_tracking.shift = 0.f;
|
||||
if (stp.shift) {
|
||||
*stp.shift = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
void shift_tracking_run(float *shift)
|
||||
{
|
||||
// store external shift variable pointer
|
||||
stp.shift = shift;
|
||||
|
||||
// compute value parameter from pos and dir
|
||||
//
|
||||
// FIXME
|
||||
// for now, assume that Z is normal to the ground
|
||||
// and shift is computed from 2D (X,Y)
|
||||
// the vertical axis in local from should be defined as well
|
||||
// for a correct 3D computation
|
||||
//
|
||||
// normal to dir = (-dir.y, dir.x) where dir is normalized
|
||||
// offset is scalar between pos and normal
|
||||
float value = (- stp.dir.y * stp.pos.x) + (stp.dir.x * stp.pos.y);
|
||||
|
||||
// shift calculation
|
||||
shift_tracking.shift = update_pid_f(&stp.pid, value, nav_dt);
|
||||
BoundAbs(shift_tracking.shift, SHIFT_TRACKING_MAXSHIFT);
|
||||
|
||||
// pilot actual value
|
||||
if (stp.shift) {
|
||||
*stp.shift = shift_tracking.shift;
|
||||
}
|
||||
}
|
||||
|
||||
void shift_tracking_update_gains(void)
|
||||
{
|
||||
set_gains_pid_f(&stp.pid, shift_tracking.kp, shift_tracking.kd, shift_tracking.ki);
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
/*
|
||||
* Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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/shift_tracking.h"
|
||||
* @author Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* pilot the nav shift variable to track an offset trajectory
|
||||
* based on the POSITION_ESTIMATE ABI message
|
||||
*/
|
||||
|
||||
#ifndef SHIFT_TRACKING_H
|
||||
#define SHIFT_TRACKING_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
struct shift_tracking_t {
|
||||
float kp; ///< proportional gain
|
||||
float kd; ///< derivative gain
|
||||
float ki; ///< integral gain
|
||||
float shift; ///< shift command
|
||||
};
|
||||
|
||||
extern struct shift_tracking_t shift_tracking;
|
||||
|
||||
/** init function
|
||||
*/
|
||||
extern void shift_tracking_init(void);
|
||||
|
||||
/** run function
|
||||
*
|
||||
* should be called in flight plan pre_call
|
||||
*
|
||||
* @param[out] shift pointer to the navigation shift to control
|
||||
*/
|
||||
extern void shift_tracking_run(float *shift);
|
||||
|
||||
/** reset function
|
||||
*
|
||||
* reset integral and offset command
|
||||
*/
|
||||
extern void shift_tracking_reset(void);
|
||||
|
||||
/** hndlers for gains update
|
||||
*/
|
||||
#define shift_tracking_SetKp(_v) { shift_tracking.kp = _v; shift_tracking_update_gains(); }
|
||||
#define shift_tracking_SetKd(_v) { shift_tracking.kd = _v; shift_tracking_update_gains(); }
|
||||
#define shift_tracking_SetKi(_v) { shift_tracking.ki = _v; shift_tracking_update_gains(); }
|
||||
extern void shift_tracking_update_gains(void);
|
||||
|
||||
#endif
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -26,6 +26,20 @@
|
||||
#ifndef DW1000_ARDUINO_H
|
||||
#define DW1000_ARDUINO_H
|
||||
|
||||
#include "std.h"
|
||||
|
||||
/** enable EKF filtering */
|
||||
extern bool dw1000_use_ekf;
|
||||
|
||||
/** process and measurements noise */
|
||||
extern float dw1000_ekf_q;
|
||||
extern float dw1000_ekf_r_dist;
|
||||
extern float dw1000_ekf_r_speed;
|
||||
/** settings handler */
|
||||
extern void dw1000_arduino_update_ekf_q(float v);
|
||||
extern void dw1000_arduino_update_ekf_r_dist(float v);
|
||||
extern void dw1000_arduino_update_ekf_r_speed(float v);
|
||||
|
||||
extern void dw1000_arduino_init(void);
|
||||
extern void dw1000_arduino_periodic(void);
|
||||
extern void dw1000_arduino_report(void);
|
||||
@@ -36,5 +50,10 @@ extern void dw1000_arduino_event(void);
|
||||
*/
|
||||
extern void dw1000_reset_heading_ref(void);
|
||||
|
||||
// when used as a GPS
|
||||
#ifndef PRIMARY_GPS
|
||||
#define PRIMARY_GPS GPS_DW1000
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -0,0 +1,198 @@
|
||||
/*
|
||||
* Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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/decawave/ekf_range.c"
|
||||
* @author Gautier Hattenberger
|
||||
* EKF based on range measurements algorithm
|
||||
*/
|
||||
|
||||
#include "ekf_range.h"
|
||||
#include <math.h>
|
||||
|
||||
void ekf_range_init(struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt)
|
||||
{
|
||||
int i,j;
|
||||
const float dt2 = dt * dt;
|
||||
const float dt3 = dt2 * dt / 2.f;
|
||||
const float dt4 = dt2 * dt2 / 4.f;
|
||||
for (i = 0; i < EKF_RANGE_DIM; i++) {
|
||||
ekf_range->state[i] = 0.f; // don't forget to call set_state before running the filter for better results
|
||||
for (j = 0; j < EKF_RANGE_DIM; j++) {
|
||||
ekf_range->P[i][j] = 0.f;
|
||||
ekf_range->Q[i][j] = 0.f;
|
||||
}
|
||||
}
|
||||
for (i = 0; i < EKF_RANGE_DIM; i += 2) {
|
||||
ekf_range->P[i][i] = P0_pos;
|
||||
ekf_range->P[i+1][i+1] = P0_speed;
|
||||
ekf_range->Q[i][i] = Q_sigma2 * dt4;
|
||||
ekf_range->Q[i+1][i] = Q_sigma2 * dt3;
|
||||
ekf_range->Q[i][i+1] = Q_sigma2 * dt3;
|
||||
ekf_range->Q[i+1][i+1] = Q_sigma2 * dt2;
|
||||
}
|
||||
ekf_range->R_dist = R_dist;
|
||||
ekf_range->R_speed = R_speed;
|
||||
ekf_range->dt = dt;
|
||||
}
|
||||
|
||||
void ekf_range_set_state(struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed)
|
||||
{
|
||||
ekf_range->state[0] = pos.x;
|
||||
ekf_range->state[1] = speed.x;
|
||||
ekf_range->state[2] = pos.y;
|
||||
ekf_range->state[3] = speed.y;
|
||||
ekf_range->state[4] = pos.z;
|
||||
ekf_range->state[5] = speed.z;
|
||||
}
|
||||
|
||||
void ekf_range_get_state(struct EKFRange *ekf_range, struct EnuCoor_f *pos, struct EnuCoor_f *speed)
|
||||
{
|
||||
pos->x = ekf_range->state[0];
|
||||
pos->y = ekf_range->state[2];
|
||||
pos->z = ekf_range->state[4];
|
||||
speed->x = ekf_range->state[1];
|
||||
speed->y = ekf_range->state[3];
|
||||
speed->z = ekf_range->state[5];
|
||||
}
|
||||
|
||||
struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range)
|
||||
{
|
||||
struct EnuCoor_f pos;
|
||||
pos.x = ekf_range->state[0];
|
||||
pos.y = ekf_range->state[2];
|
||||
pos.z = ekf_range->state[4];
|
||||
return pos;
|
||||
}
|
||||
|
||||
struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range)
|
||||
{
|
||||
struct EnuCoor_f speed;
|
||||
speed.x = ekf_range->state[1];
|
||||
speed.y = ekf_range->state[3];
|
||||
speed.z = ekf_range->state[5];
|
||||
return speed;
|
||||
}
|
||||
|
||||
void ekf_range_update_noise(struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed)
|
||||
{
|
||||
int i;
|
||||
const float dt = ekf_range->dt;
|
||||
const float dt2 = dt * dt;
|
||||
const float dt3 = dt2 * dt / 2.f;
|
||||
const float dt4 = dt2 * dt2 / 4.f;
|
||||
for (i = 0; i < EKF_RANGE_DIM; i += 2) {
|
||||
ekf_range->Q[i][i] = Q_sigma2 * dt4;
|
||||
ekf_range->Q[i+1][i] = Q_sigma2 * dt3;
|
||||
ekf_range->Q[i][i+1] = Q_sigma2 * dt3;
|
||||
ekf_range->Q[i+1][i+1] = Q_sigma2 * dt2;
|
||||
}
|
||||
ekf_range->R_dist = R_dist;
|
||||
ekf_range->R_speed = R_speed;
|
||||
}
|
||||
|
||||
/** propagate dynamic model
|
||||
*
|
||||
* F = [ 1 dt 0 0 0 0
|
||||
* 0 1 0 0 0 0
|
||||
* 0 0 1 dt 0 0
|
||||
* 0 0 0 1 0 0
|
||||
* 0 0 0 0 1 dt
|
||||
* 0 0 0 0 0 1 ]
|
||||
*/
|
||||
void ekf_range_predict(struct EKFRange *ekf_range)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < EKF_RANGE_DIM; i += 2) {
|
||||
// kinematic equation of the dynamic model X = F*X
|
||||
ekf_range->state[i] += ekf_range->state[i+1] * ekf_range->dt;
|
||||
// propagate covariance P = F*P*Ft + Q
|
||||
// since F is diagonal by block, P can be updated by block here as well
|
||||
// let's unroll the matrix operations as it is simple
|
||||
const float d_dt = ekf_range->P[i+1][i+1] * ekf_range->dt;
|
||||
ekf_range->P[i][i] += ekf_range->P[i+1][i] * ekf_range->dt
|
||||
+ ekf_range->dt * (ekf_range->P[i][i+1] + d_dt) + ekf_range->Q[i][i];
|
||||
ekf_range->P[i][i+1] += d_dt + ekf_range->Q[i][i+1];
|
||||
ekf_range->P[i+1][i] += d_dt + ekf_range->Q[i+1][i];
|
||||
ekf_range->P[i+1][i+1] += ekf_range->Q[i+1][i+1];
|
||||
}
|
||||
}
|
||||
|
||||
/** correction step
|
||||
*
|
||||
* K = PHt(HPHt+R)^1
|
||||
* X = X + K(z-h(X))
|
||||
* P = (I-KH)P
|
||||
*/
|
||||
void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor)
|
||||
{
|
||||
const float dx = ekf_range->state[0] - anchor.x;
|
||||
const float dy = ekf_range->state[2] - anchor.y;
|
||||
const float dz = ekf_range->state[4] - anchor.z;
|
||||
const float norm = sqrtf(dx * dx + dy * dy + dz * dz);
|
||||
// build measurement error
|
||||
const float res = dist - norm;
|
||||
// build Jacobian of observation model for anchor i
|
||||
float Hi[] = { dx / norm, 0.f, dy / norm, 0.f, dz / norm, 0.f };
|
||||
// compute kalman gain K = P*Ht (H*P*Ht + R)^-1
|
||||
// S = H*P*Ht + R
|
||||
const float S =
|
||||
Hi[0] * Hi[0] * ekf_range->P[0][0] +
|
||||
Hi[2] * Hi[2] * ekf_range->P[2][2] +
|
||||
Hi[4] * Hi[4] * ekf_range->P[4][4] +
|
||||
Hi[0] * Hi[2] * (ekf_range->P[0][2] + ekf_range->P[2][0]) +
|
||||
Hi[0] * Hi[4] * (ekf_range->P[0][4] + ekf_range->P[4][0]) +
|
||||
Hi[2] * Hi[4] * (ekf_range->P[2][4] + ekf_range->P[4][2]) +
|
||||
ekf_range->R_dist;
|
||||
if (fabsf(S) < 1e-5) {
|
||||
return; // don't inverse S if it is too small
|
||||
}
|
||||
// finally compute gain and correct state
|
||||
float K[6];
|
||||
for (int i = 0; i < 6; i++) {
|
||||
K[i] = (Hi[0] * ekf_range->P[i][0] + Hi[2] * ekf_range->P[i][2] + Hi[4] * ekf_range->P[i][4]) / S;
|
||||
ekf_range->state[i] += K[i] * res;
|
||||
}
|
||||
// precompute K*H and store current P
|
||||
float KH_tmp[6][6];
|
||||
float P_tmp[6][6];
|
||||
for (int i = 0; i < 6; i++) {
|
||||
for (int j = 0; j < 6; j++) {
|
||||
KH_tmp[i][j] = K[i] * Hi[j];
|
||||
P_tmp[i][j] = ekf_range->P[i][j];
|
||||
}
|
||||
}
|
||||
// correct covariance P = (I-K*H)*P = P - K*H*P
|
||||
for (int i = 0; i < 6; i++) {
|
||||
for (int j = 0; j < 6; j++) {
|
||||
for (int k = 0; k < 6; k++) {
|
||||
ekf_range->P[i][j] -= KH_tmp[i][k] * P_tmp[k][j];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void ekf_range_update_speed(struct EKFRange *ekf_range, float speed, uint8_t type)
|
||||
{
|
||||
(void) ekf_range;
|
||||
(void) speed;
|
||||
(void) type;
|
||||
// TODO
|
||||
}
|
||||
|
||||
@@ -0,0 +1,131 @@
|
||||
/*
|
||||
* Copyright (C) 2018 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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/decawave/ekf_range.h"
|
||||
* @author Gautier Hattenberger
|
||||
* EKF based on range measurements algorithm
|
||||
*/
|
||||
|
||||
#ifndef EKF_RANGE_H
|
||||
#define EKF_RANGE_H
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
|
||||
#define EKF_RANGE_DIM 6
|
||||
|
||||
/** EKF_range structure
|
||||
*
|
||||
* state vector: X = [ x xd y yd z zd ]'
|
||||
* command vector: U = 0 (constant velocity model)
|
||||
* dynamic model: basic kinematic model x_k+1 = x_k + xd_k * dt
|
||||
* measures: distance between (fixed and known) anchors and UAV
|
||||
*
|
||||
* */
|
||||
struct EKFRange {
|
||||
float state[EKF_RANGE_DIM]; ///< state vector
|
||||
float P[EKF_RANGE_DIM][EKF_RANGE_DIM]; ///< covariance matrix
|
||||
float Q[EKF_RANGE_DIM][EKF_RANGE_DIM]; ///< process noise matrix
|
||||
float R_dist; ///< measurement noise on distances (assumed the same for all anchors)
|
||||
float R_speed; ///< measurement noise on speed (assumed the same for all axis)
|
||||
float dt; ///< prediction step (in seconds)
|
||||
};
|
||||
|
||||
/** Init EKF_range internal struct
|
||||
*
|
||||
* @param[in] ekf_range EKFRange structure
|
||||
* @param[in] P0_pos initial covariance on position
|
||||
* @param[in] P0_speed initial covariance on speed
|
||||
* @param[in] Q_sigma2 process noise
|
||||
* @param[in] R_dist measurement noise on distance
|
||||
* @param[in] R_speed measurement noise on speed
|
||||
* @param[in] dt prediction time step in seconds
|
||||
*/
|
||||
extern void ekf_range_init(struct EKFRange *ekf_range, float P0_pos, float P0_speed, float Q_sigma2, float R_dist, float R_speed, float dt);
|
||||
|
||||
/** Set initial state vector
|
||||
*
|
||||
* This function should be called after initialization of the ekf struct and before
|
||||
* running the filter for better results and faster convergence
|
||||
*
|
||||
* @param[in] ekf_range EKFRange structure
|
||||
* @param[in] pos initial position
|
||||
* @param[in] speed initial speed
|
||||
*/
|
||||
extern void ekf_range_set_state(struct EKFRange *ekf_range, struct EnuCoor_f pos, struct EnuCoor_f speed);
|
||||
|
||||
/** Get current state
|
||||
*
|
||||
* @param[in] ekf_range EKFRange structure
|
||||
* @param[out] pos current position
|
||||
* @param[out] speed current speed
|
||||
*/
|
||||
extern void ekf_range_get_state(struct EKFRange *ekf_range, struct EnuCoor_f *pos, struct EnuCoor_f *speed);
|
||||
|
||||
/** Get current pos
|
||||
*
|
||||
* @param[in] ekf_range EKFRange structure
|
||||
* @return current position
|
||||
*/
|
||||
extern struct EnuCoor_f ekf_range_get_pos(struct EKFRange *ekf_range);
|
||||
|
||||
/** Get current speed
|
||||
*
|
||||
* @param[in] ekf_range EKFRange structure
|
||||
* @return current speed
|
||||
*/
|
||||
extern struct EnuCoor_f ekf_range_get_speed(struct EKFRange *ekf_range);
|
||||
|
||||
/** Update process and measurement noises
|
||||
*
|
||||
* @param[in] ekf_range EKFRange structure
|
||||
* @param[in] Q_sigma2 process noise
|
||||
* @param[in] R_dist measurement noise on distance
|
||||
* @param[in] R_speed measurement noise on speed
|
||||
*/
|
||||
extern void ekf_range_update_noise(struct EKFRange *ekf_range, float Q_sigma2, float R_dist, float R_speed);
|
||||
|
||||
/** Prediction step
|
||||
*
|
||||
* @param[in] ekf_range EKFRange structure
|
||||
*/
|
||||
extern void ekf_range_predict(struct EKFRange *ekf_range);
|
||||
|
||||
/** Update step based on each new distance data
|
||||
*
|
||||
* @param[in] ekf_range EKFRange structure
|
||||
* @param[in] dist new distance measurement
|
||||
* @param[in] anchor position of the anchor from which the distance is measured
|
||||
*/
|
||||
extern void ekf_range_update_dist(struct EKFRange *ekf_range, float dist, struct EnuCoor_f anchor);
|
||||
|
||||
/** Update step based on speed measure
|
||||
*
|
||||
* @param[in] ekf_range EKFRange structure
|
||||
* @param[in] speed new speed measurement
|
||||
* @param[in] type 1: horizontal ground speed norm, 2: vertical ground speed norm, 3: 3D ground speed norm
|
||||
*/
|
||||
extern void ekf_range_update_speed(struct EKFRange *ekf_range, float speed, uint8_t type);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -110,6 +110,7 @@ int trilateration_compute(struct Anchor *anchors, struct EnuCoor_f *pos)
|
||||
pos->x = P[0][0] + tmp[0] * Ex[0] + tmp[1] * Ey[0] + tmp[2] * Ez[0];
|
||||
pos->y = P[0][1] + tmp[0] * Ex[1] + tmp[1] * Ey[1] + tmp[2] * Ez[1];
|
||||
pos->z = P[0][2] + tmp[0] * Ex[2] + tmp[1] * Ey[2] + tmp[2] * Ez[2];
|
||||
pos->z = fabsf(pos->z); // in case the base is not matching, keep positive z
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -36,6 +36,7 @@ struct Anchor {
|
||||
float time; ///< time of the last received data
|
||||
struct EnuCoor_f pos; ///< position of the anchor
|
||||
uint16_t id; ///< anchor ID
|
||||
bool updated; ///< new data available
|
||||
};
|
||||
|
||||
/** Init internal trilateration structures
|
||||
|
||||
@@ -29,6 +29,8 @@
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "subsystems/abi.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
|
||||
int jevois_mapping_setting;
|
||||
@@ -72,10 +74,42 @@ struct jevois_t {
|
||||
uint8_t idx; // temp buffer index
|
||||
uint8_t n; // temp coordinates/dimension index
|
||||
struct jevois_msg_t msg; // last decoded message
|
||||
bool data_available; // new data to report
|
||||
};
|
||||
|
||||
struct jevois_t jevois;
|
||||
|
||||
// reporting function, send telemetry message
|
||||
void jevois_report(void)
|
||||
{
|
||||
if (jevois.data_available == false) {
|
||||
// no new data, return
|
||||
return;
|
||||
}
|
||||
|
||||
float quat[4] = {
|
||||
jevois.msg.quat.qi,
|
||||
jevois.msg.quat.qx,
|
||||
jevois.msg.quat.qy,
|
||||
jevois.msg.quat.qz
|
||||
};
|
||||
uint8_t len = strlen(jevois.msg.id);
|
||||
char none[] = "None";
|
||||
char *id = jevois.msg.id;
|
||||
if (len == 0) {
|
||||
id = none;
|
||||
len = 4;
|
||||
}
|
||||
DOWNLINK_SEND_JEVOIS(DefaultChannel, DefaultDevice,
|
||||
&jevois.msg.type,
|
||||
len, id,
|
||||
&jevois.msg.nb,
|
||||
Max(jevois.msg.nb,1), jevois.msg.coord,
|
||||
jevois.msg.dim,
|
||||
quat);
|
||||
jevois.data_available = false;
|
||||
}
|
||||
|
||||
// initialization
|
||||
void jevois_init(void)
|
||||
{
|
||||
@@ -86,12 +120,17 @@ void jevois_init(void)
|
||||
jevois.state = JV_SYNC;
|
||||
jevois.idx = 0;
|
||||
jevois.n = 0;
|
||||
jevois.data_available = false;
|
||||
memset(jevois.buf, 0, JEVOIS_MAX_LEN);
|
||||
}
|
||||
|
||||
// send specific message if requested
|
||||
static void jevois_send_message(void)
|
||||
{
|
||||
#if JEVOIS_SEND_MSG
|
||||
// send pprzlink JEVOIS message
|
||||
jevois_report();
|
||||
#endif
|
||||
#if JEVOIS_SEND_FOLLOW_TARGET
|
||||
float cam_heading = (JEVOIS_HFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[0]);
|
||||
float cam_height = (JEVOIS_VFOV / (2.f * JEVOIS_NORM)) * (float)(jevois.msg.coord[1]);
|
||||
@@ -200,7 +239,7 @@ static void jevois_parse(struct jevois_t *jv, char c)
|
||||
if (JEVOIS_CHECK_DELIM(c)) {
|
||||
jv->buf[jv->idx] = '\0'; // end string
|
||||
jv->msg.coord[jv->n++] = (int16_t)atoi(jv->buf); // store value
|
||||
if (jv->n == 2 * jv->msg.nb) {
|
||||
if (jv->n == jv->msg.nb) {
|
||||
// got all coordinates, go to next state
|
||||
jv->n = 0; // reset number of received elements
|
||||
jv->idx = 0; // reset index
|
||||
@@ -257,10 +296,10 @@ static void jevois_parse(struct jevois_t *jv, char c)
|
||||
case JV_QUAT:
|
||||
if (JEVOIS_CHECK_DELIM(c)) {
|
||||
jv->buf[jv->idx] = '\0';
|
||||
float q = 0.f;//(float) atof(jv->buf);
|
||||
float q = (float)atof(jv->buf);
|
||||
switch (jv->n) {
|
||||
case 0:
|
||||
jv->msg.quat.qi = q; // TODO check quaternion order
|
||||
jv->msg.quat.qi = q;
|
||||
break;
|
||||
case 1:
|
||||
jv->msg.quat.qx = q;
|
||||
@@ -310,6 +349,7 @@ static void jevois_parse(struct jevois_t *jv, char c)
|
||||
jv->msg.extra);
|
||||
// also send specific messages if needed
|
||||
jevois_send_message();
|
||||
jv->data_available = true;
|
||||
jv->state = JV_SYNC;
|
||||
break;
|
||||
default:
|
||||
@@ -331,7 +371,7 @@ void jevois_event(void)
|
||||
}
|
||||
|
||||
// utility function to send a string
|
||||
static void send_string(char *s)
|
||||
void jevois_send_string(char *s)
|
||||
{
|
||||
uint8_t i = 0;
|
||||
while (s[i]) {
|
||||
@@ -344,9 +384,9 @@ void jevois_stream(bool activate)
|
||||
{
|
||||
jevois_stream_setting = activate;
|
||||
if (activate) {
|
||||
send_string("streamon\r\n");
|
||||
jevois_send_string("streamon\r\n");
|
||||
} else {
|
||||
send_string("streamoff\r\n");
|
||||
jevois_send_string("streamoff\r\n");
|
||||
}
|
||||
}
|
||||
|
||||
@@ -354,13 +394,13 @@ void jevois_setmapping(int number)
|
||||
{
|
||||
jevois_mapping_setting = number;
|
||||
jevois_stream(false);
|
||||
send_string("setmapping ");
|
||||
jevois_send_string("setmapping ");
|
||||
char s[4];
|
||||
#ifndef SITL
|
||||
itoa(number, s, 10);
|
||||
#endif
|
||||
send_string(s);
|
||||
send_string("\r\n");
|
||||
jevois_send_string(s);
|
||||
jevois_send_string("\r\n");
|
||||
jevois_stream(true);
|
||||
}
|
||||
|
||||
|
||||
@@ -61,6 +61,13 @@
|
||||
|
||||
extern void jevois_init(void);
|
||||
extern void jevois_event(void);
|
||||
extern void jevois_report(void);
|
||||
|
||||
/**
|
||||
* Generic function to send a string command to Jevois
|
||||
* @param[in] s string command to send
|
||||
*/
|
||||
extern void jevois_send_string(char *s);
|
||||
|
||||
/** Start and stop streaming
|
||||
* @param[in] activate enable or disable streaming
|
||||
|
||||
@@ -203,6 +203,7 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
|
||||
struct Int32Vect3 accel_body, accel_ned;
|
||||
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&body_to_imu);
|
||||
int32_rmat_transp_vmult(&accel_body, body_to_imu_rmat, accel);
|
||||
stateSetAccelBody_i(&accel_body);
|
||||
struct Int32RMat *ned_to_body_rmat = stateGetNedToBodyRMat_i();
|
||||
int32_rmat_transp_vmult(&accel_ned, ned_to_body_rmat, &accel_body);
|
||||
accel_ned.z += ACCEL_BFP_OF_REAL(9.81);
|
||||
|
||||
+1
-1
Submodule sw/ext/pprzlink updated: 61192d861f...d9173a6dc9
Reference in New Issue
Block a user