Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

Nederdrone ctrl scheduling + moving target app (#2957)

Browse Source 
* Nederdrone ctrl scheduling + moving target app

- Nederdrone airframe files
- Ctrl effectiveness scheduling
- Target position communication and planning
- Approach to a moving target guidance

* Update conf

* fix compilation problems

* flight plan compatible new GCS

* remove module test

Co-authored-by: Freek van Tienen <freek.v.tienen@gmail.com>
This commit is contained in:
Ewoud Smeur
2022-12-16 16:02:16 +01:00
committed by GitHub
parent 3502cab856
commit dc998be91d
23 changed files with 3136 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/nederdrone4.xml
+5 -1
View File
@@ -73,6 +73,10 @@
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="approach_moving_target">
<define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
</module>
<module name="actuators" type="uavcan">
<configure name="UAVCAN_USE_CAN1" value="TRUE"/>
<configure name="UAVCAN_USE_CAN2" value="TRUE"/>
@@ -84,7 +88,7 @@
<module name="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/>
<module name="ins" type="ekf2" />
<module name="air_data"/>
<!-- Internal MAG -->
conf/airframes/tudelft/nederdrone5.xml
+6 -2
View File
@@ -70,6 +70,10 @@
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="approach_moving_target">
<define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
</module>
<module name="actuators" type="uavcan">
<configure name="UAVCAN_USE_CAN1" value="TRUE"/>
<configure name="UAVCAN_USE_CAN2" value="TRUE"/>
@@ -80,7 +84,7 @@
<module name="stabilization" type="indi_simple"/>
<module name="stabilization" type="rate_indi"/>
<module name="ins" type="ekf2" />
<module name="air_data"/>
<!-- Internal MAG -->
@@ -193,7 +197,7 @@
<set servo="MOTOR_10" value="($th_hold? -9600 : motor_mixing.commands[9])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : motor_mixing.commands[10])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : motor_mixing.commands[11])"/>
<!-- Removed ApplyDiff for differential control -->
<set servo="AIL_1" value="-2*@PITCH + ( 2*@YAW)"/>
<set servo="AIL_2" value="-2*@PITCH + (- 2*@YAW)"/>
conf/airframes/tudelft/nederdrone6.xml
+394
View File
@@ -0,0 +1,394 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- This is a Nedderdrone with Trailing edge motors
* Airframe: TUD00???
* Autopilot: Pixhawk 4
* Actuators: 12x T-Motor ESC + Motors and 8x Servos (all CAN)
* Datalink: Herelink
* GPS: UBlox F9P
* RC: SBUS Crossfire
-->
<airframe name="Neddrone6">
<description>Neddrone6</description>
<firmware name="rotorcraft">
<target name="ap" board="px4fmu_5.0_chibios">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<configure name="FLASH_MODE" value="SWD"/>
<define name="USE_BARO_BOARD" value="1"/>
<module name="radio_control" type="sbus">
<configure name="SBUS_PORT" value="UART3"/>
</module>
<!-- Forward FuelCell data back to the GCS -->
<!--module name="generic_uart_sensor"/-->
<!-- Logger -->
<module name="tlsf"/>
<module name="pprzlog"/>
<module name="logger" type="sd_chibios"/>
<module name="flight_recorder"/>
<!-- Airspeed sensors -->
<module name="airspeed" type="ms45xx_i2c">
<define name="USE_I2C4"/>
<define name="MS45XX_I2C_DEV" value="i2c4"/>
</module>
<!--module name="airspeed" type="uavcan"/-->
<!-- Monitoring -->
<module name="sys_mon"/>
<!--module name="status_nederdrone"/-->
<define name="ADC_CURRENT_DISABLE" value="TRUE"/>
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="RADIO_AUX2"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<!-- <module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</module> -->
<!--module name="ins" type="extended">
<define name="INS_USE_GPS_ALT" value="1"/>
<define name="INS_USE_GPS_ALT_SPEED" value="1"/>
<define name="INS_VFF_R_GPS" value="0.01"/>
</module-->
</target>
<target name="nps" board="pc">
<!-- <module name="eff_scheduling_nederdrone_dummy"/> -->
<module name="radio_control" type="datalink"/>
<module name="fdm" type="jsbsim"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
</module>
<!--Not dealing with these in the simulation-->
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
</target>
<module name="eff_scheduling_nederdrone"/>
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="approach_moving_target">
<define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
</module>
<module name="ins" type="ekf2" />
<module name="actuators" type="uavcan">
<configure name="UAVCAN_USE_CAN1" value="TRUE"/>
<configure name="UAVCAN_USE_CAN2" value="TRUE"/>
</module>
<module name="imu" type="mpu6000"/>
<!--module name="gps" type="datalink"/-->
<module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module>
<module name="stabilization" type="indi">
<configure name="INDI_NUM_ACT" value="8"/>
<define name="TILT_TWIST_CTRL" value="TRUE"/>
</module>
<module name="stabilization" type="rate_indi"/>
<module name="air_data">
<define name="AIR_DATA_BARO_DIFF_ID" value="MS45XX_SENDER_ID"/> <!-- UAVCAN_SENDER_ID -->
<define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE" />
</module>
<!-- Internal MAG -->
<!--module name="mag_ist8310">
<define name="MODULE_IST8310_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_IST8310_I2C_DEV" value="I2C3"/>
</module-->
<!-- External MAG on GPS -->
<module name="mag_lis3mdl">
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
<define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
<define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
</module>
<!--module name="lidar" type="tfmini">
<configure name="TFMINI_PORT" value="UART4"/>
<configure name="USE_TFMINI_AGL" value="FALSE"/>
</module-->
<module name="guidance" type="indi_hybrid">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="0.5"/>
<define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
<define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="17.0"/>
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
<define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
<!--define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_45" value="-500.0"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD" value="-1600.0"/-->
<!-- <define name="GUIDANCE_INDI_FILTER_CUTOFF" value="0.5"/> -->
<define name="GUIDANCE_INDI_LINE_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE" value="1500"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE_FWD" value="1500"/>
<define name="GUIDANCE_INDI_LIFTD_P50" value="6.0"/>
<define name="GUIDANCE_INDI_LIFTD_P80" value="10.0"/>
<define name="GUIDANCE_INDI_LIFTD_ASQ" value="0.15"/>
</module>
<module name="motor_mixing"/>
</firmware>
<!-- CAN BUS 1 (Front Wing) -->
<servos driver="Uavcan1">
<servo name="MOTOR_1" no="0" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_2" no="1" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_3" no="2" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_4" no="3" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_5" no="4" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_6" no="5" min="-8191" neutral="1500" max="8191"/>
<servo name="AIL_1" no="6" min="-6000" neutral="0" max="6000"/>
<servo name="AIL_2" no="7" min="6000" neutral="0" max="-6000"/>
</servos>
<!-- CAN BUS 2 (Back Wing) -->
<servos driver="Uavcan2">
<servo name="MOTOR_7" no="0" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_8" no="1" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_9" no="2" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_10" no="3" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_11" no="4" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_12" no="5" min="0" neutral="600" max="8191"/>
<servo name="AIL_3" no="6" min="-6000" neutral="0" max="6000"/>
<servo name="AIL_4" no="7" min="6000" neutral="0" max="-6000"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="-300"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THRUST" value="@THROTTLE" />
<set command="ROLL" value="@YAW" />
<set command="PITCH" value="@PITCH/2" />
<set command="YAW" value="-@ROLL/4" />
</rc_commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<!-- frontleft left (CCW), frontleft mid (CW), frontleft right (CCW), frontright left (CW), frontright mid (CCW), frontright right (CW) -->
<!-- backleft left (CW), backleft mid (CCW), backleft right (CW), backright left (CCW), backright mid (CW), backright right (CCW) -->
<define name="NB_MOTOR" value="12"/>
<define name="SCALE" value="256"/>
<define name="ROLL_COEF" value="{256, 157, 56, -56, -157, -256, 256, 157, 56, -56, -157, -256}"/>
<define name="PITCH_COEF" value="{256, 256, 256, 256, 256, 256, -256, -256, -256, -256, -256, -256}"/>
<define name="YAW_COEF" value="{251, -256, 252, -252, 256, -251, -256, 252, -254, 254, -252, 256}"/>
<!--<define name="YAW_COEF" value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
<define name="THRUST_COEF" value="{256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256}"/>
</section>
<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<!-- Tip props always at 80% -->
<call fun="actuators_pprz[8] = (Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*70*sched_ratio_tip_props);"/>
<set servo="MOTOR_1" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_7" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : actuators_pprz[3])"/>
<!-- Removed ApplyDiff for differential control -->
<set servo="AIL_1" value="($th_hold? 9600 : actuators_pprz[4])"/>
<set servo="AIL_2" value="($th_hold? 9600 : actuators_pprz[5])"/>
<set servo="AIL_3" value="actuators_pprz[6]"/>
<set servo="AIL_4" value="actuators_pprz[7]"/>
</command_laws>
<section name="MISC">
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 18.028 * adc)"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<!-- Basic navigation settings -->
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
<define name="NAV_DESCEND_VSPEED" value="-0.7"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<!-- Settings for circle -->
<define name="DEFAULT_CIRCLE_RADIUS" value="700"/>
<define name="NAV_CARROT_DIST" value="200"/>
<!-- Avoid GPS loss behavior when having RC or datalink -->
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
</section>
<section name="FORWARD">
<!--The Nederdrone uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- This is the pitch angle that the Nederdrone will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/>
<!-- For RC coordinated turns, lower because the yawing was too slow -->
<define name="MAX_FWD_SPEED" value="20.0"/>
<!-- For hybrid guidance -->
<define name="MAX_AIRSPEED" value="20.0"/>
<!-- Enable airspeed measurements -->
<define name="USE_AIRSPEED" value="TRUE"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Rotate the IMU -->
<define name="MPU_CHAN_X" value="0"/>
<define name="MPU_CHAN_Y" value="2"/>
<define name="MPU_CHAN_Z" value="1"/>
<define name="MPU_X_SIGN" value="-1"/>
<define name="MPU_Y_SIGN" value="1"/>
<define name="MPU_Z_SIGN" value="1"/>
<!-- Calibrated with body only (29-06-2022) -->
<define name="ACCEL_X_NEUTRAL" value="-45"/>
<define name="ACCEL_Y_NEUTRAL" value="13"/>
<define name="ACCEL_Z_NEUTRAL" value="-257"/>
<define name="ACCEL_X_SENS" value="4.895116343046987" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.83753590244878" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.670246691763502" integer="16"/>
<!-- Calibrated at valkenburg (15-07-2022) -->
<define name="MAG_X_NEUTRAL" value="432"/>
<define name="MAG_Y_NEUTRAL" value="1679"/>
<define name="MAG_Z_NEUTRAL" value="3433"/>
<define name="MAG_X_SENS" value="0.6409337832137453" integer="16"/>
<define name="MAG_Y_SENS" value="0.652074798089911" integer="16"/>
<define name="MAG_Z_SENS" value="0.6537789401490781" integer="16"/>
<!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="72.3" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="80." unit="deg"/>
<define name="SP_MAX_R" value="180." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="60" unit="deg"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<define name="G1_ROLL" value="{ 0.8, -0.8, 1.6, -1.6, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 0.95, 0.95, -1.425, -1.425, -0.5, -0.5, -0.5, -0.5}"/>
<define name="G1_YAW" value="{ 0.00, 0.0, -0.00, 0.00, 0.4, -0.4, -0.4, 0.4}"/>
<define name="G1_THRUST" value="{ -0.3, -0.3, -0.45, -0.45, 0.0, 0.0, 0.0, 0.0}"/>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{0, 0, 0, 0, 0, 0, 0, 0}"/>
<!-- Forward gains -->
<define name="G1_ROLL_FWD" value="{ 0.5, -0.5, 0.96, -0.96, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH_FWD" value="{ 0.6, 0.6, -0.86, -0.86, -1.4, -1.4, -1.4, -1.4}"/>
<define name="G1_YAW_FWD" value="{ 0.0, 0.0, 0.0, 0.0, 0.94, -0.94, -0.94, 0.94}"/>
<define name="G1_THRUST_FWD" value="{-0.18 -0.18, -0.27, -0.27, 0.0, 0.0, 0.0, 0.0}"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="30.0"/>
<define name="REF_ERR_R" value="20.0"/>
<define name="REF_RATE_P" value="6.0"/>
<define name="REF_RATE_Q" value="6.0"/>
<define name="REF_RATE_R" value="6.0"/>
<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="180.0" unit="deg/s"/>
<!-- Maximum rate setpoint in rate control mode -->
<define name="MAX_RATE" value="3.0" unit="rad/s"/>
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="1.5"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
<define name="FILT_CUTOFF_R" value="4."/>
<define name="FILT_CUTOFF_P" value="20."/>
<define name="FILT_CUTOFF_Q" value="20."/>
<!-- first order actuator dynamics -->
<!--define name="ACT_DYN" value="{0.009, 0.009, 0.009, 0.009, 0.05, 0.05, 0.05, 0.05}"/-->
<define name="ACT_DYN" value="{0.028, 0.028, 0.028, 0.028, 0.05, 0.05, 0.05, 0.05}"/>
<define name="ACT_RATE_LIMIT" value="{9600, 9600, 9600, 9600, 170, 170, 170, 170}"/>
<define name="ACT_IS_SERVO" value="{0, 0, 0, 0, 1, 1, 1, 1}"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="310"/>
<define name="HOVER_KD" value="130"/>
<define name="HOVER_KI" value="10"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.42"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="28" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="60"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="0"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="a0,a1,a2,a3,a4,a5,a6,a7,a8" type="string[]"/>
<define name="JSBSIM_MODEL" value="nederdrone4_tem" type="string"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="COMMANDS_NB" value="9"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
<define name="DEBUG_SPEED_SP" value="false"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
</airframe>
conf/airframes/tudelft/nederdrone7.xml
+393
View File
@@ -0,0 +1,393 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- This is a Nedderdrone with Trailing edge motors
* Airframe: TUD00???
* Autopilot: Pixhawk 4
* Actuators: 12x T-Motor ESC + Motors and 8x Servos (all CAN)
* Datalink: Herelink
* GPS: UBlox F9P
* RC: SBUS Crossfire
-->
<airframe name="Neddrone7">
<description>Neddrone7</description>
<firmware name="rotorcraft">
<target name="ap" board="px4fmu_5.0_chibios">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<configure name="FLASH_MODE" value="SWD"/>
<define name="USE_BARO_BOARD" value="1"/>
<module name="radio_control" type="sbus">
<configure name="SBUS_PORT" value="UART3"/>
</module>
<!-- Forward FuelCell data back to the GCS -->
<!--module name="generic_uart_sensor"/-->
<!-- Logger -->
<module name="tlsf"/>
<module name="pprzlog"/>
<module name="logger" type="sd_chibios"/>
<module name="flight_recorder"/>
<!-- Airspeed sensors -->
<module name="airspeed" type="ms45xx_i2c">
<define name="USE_I2C4"/>
<define name="MS45XX_I2C_DEV" value="i2c4"/>
</module>
<!--module name="airspeed" type="uavcan"/-->
<!-- Monitoring -->
<module name="sys_mon"/>
<!--module name="status_nederdrone"/-->
<define name="ADC_CURRENT_DISABLE" value="TRUE"/>
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="RADIO_AUX2"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<!-- <module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</module> -->
<!--module name="ins" type="extended">
<define name="INS_USE_GPS_ALT" value="1"/>
<define name="INS_USE_GPS_ALT_SPEED" value="1"/>
<define name="INS_VFF_R_GPS" value="0.01"/>
</module-->
</target>
<target name="nps" board="pc">
<module name="radio_control" type="datalink"/>
<module name="fdm" type="jsbsim"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
</module>
<!--Not dealing with these in the simulation-->
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
</target>
<module name="eff_scheduling_nederdrone"/>
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="approach_moving_target">
<define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
</module>
<module name="ins" type="ekf2" />
<module name="actuators" type="uavcan">
<configure name="UAVCAN_USE_CAN1" value="TRUE"/>
<configure name="UAVCAN_USE_CAN2" value="TRUE"/>
</module>
<module name="imu" type="mpu6000"/>
<!--module name="gps" type="datalink"/-->
<module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module>
<module name="stabilization" type="indi">
<configure name="INDI_NUM_ACT" value="8"/>
<define name="TILT_TWIST_CTRL" value="TRUE"/>
</module>
<module name="stabilization" type="rate_indi"/>
<module name="air_data">
<define name="AIR_DATA_BARO_DIFF_ID" value="MS45XX_SENDER_ID"/> <!-- UAVCAN_SENDER_ID -->
<define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE" />
</module>
<!-- Internal MAG -->
<!--module name="mag_ist8310">
<define name="MODULE_IST8310_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_IST8310_I2C_DEV" value="I2C3"/>
</module-->
<!-- External MAG on GPS -->
<module name="mag_lis3mdl">
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
<define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
<define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
</module>
<!--module name="lidar" type="tfmini">
<configure name="TFMINI_PORT" value="UART4"/>
<configure name="USE_TFMINI_AGL" value="FALSE"/>
</module-->
<module name="guidance" type="indi_hybrid">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="0.5"/>
<define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
<define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="17.0"/>
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
<define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
<!--define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_45" value="-500.0"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD" value="-1600.0"/-->
<!-- <define name="GUIDANCE_INDI_FILTER_CUTOFF" value="0.5"/> -->
<define name="GUIDANCE_INDI_LINE_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE" value="1500"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE_FWD" value="1500"/>
<define name="GUIDANCE_INDI_LIFTD_P50" value="6.0"/>
<define name="GUIDANCE_INDI_LIFTD_P80" value="10.0"/>
<define name="GUIDANCE_INDI_LIFTD_ASQ" value="0.15"/>
</module>
<module name="motor_mixing"/>
</firmware>
<!-- CAN BUS 1 (Front Wing) -->
<servos driver="Uavcan1">
<servo name="MOTOR_1" no="0" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_2" no="1" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_3" no="2" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_4" no="3" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_5" no="4" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_6" no="5" min="-8191" neutral="1500" max="8191"/>
<servo name="AIL_1" no="6" min="-6000" neutral="0" max="6000"/>
<servo name="AIL_2" no="7" min="6000" neutral="0" max="-6000"/>
</servos>
<!-- CAN BUS 2 (Back Wing) -->
<servos driver="Uavcan2">
<servo name="MOTOR_7" no="0" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_8" no="1" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_9" no="2" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_10" no="3" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_11" no="4" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_12" no="5" min="0" neutral="600" max="8191"/>
<servo name="AIL_3" no="6" min="-6000" neutral="0" max="6000"/>
<servo name="AIL_4" no="7" min="6000" neutral="0" max="-6000"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="-300"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THRUST" value="@THROTTLE" />
<set command="ROLL" value="@YAW" />
<set command="PITCH" value="@PITCH/2" />
<set command="YAW" value="-@ROLL/4" />
</rc_commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<!-- frontleft left (CCW), frontleft mid (CW), frontleft right (CCW), frontright left (CW), frontright mid (CCW), frontright right (CW) -->
<!-- backleft left (CW), backleft mid (CCW), backleft right (CW), backright left (CCW), backright mid (CW), backright right (CCW) -->
<define name="NB_MOTOR" value="12"/>
<define name="SCALE" value="256"/>
<define name="ROLL_COEF" value="{256, 157, 56, -56, -157, -256, 256, 157, 56, -56, -157, -256}"/>
<define name="PITCH_COEF" value="{256, 256, 256, 256, 256, 256, -256, -256, -256, -256, -256, -256}"/>
<define name="YAW_COEF" value="{251, -256, 252, -252, 256, -251, -256, 252, -254, 254, -252, 256}"/>
<!--<define name="YAW_COEF" value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
<define name="THRUST_COEF" value="{256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256}"/>
</section>
<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<!-- Tip props always at 80% -->
<call fun="actuators_pprz[8] = (Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*70*sched_ratio_tip_props);"/>
<set servo="MOTOR_1" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_7" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : actuators_pprz[3])"/>
<!-- Removed ApplyDiff for differential control -->
<set servo="AIL_1" value="($th_hold? 9600 : actuators_pprz[4])"/>
<set servo="AIL_2" value="($th_hold? 9600 : actuators_pprz[5])"/>
<set servo="AIL_3" value="actuators_pprz[6]"/>
<set servo="AIL_4" value="actuators_pprz[7]"/>
</command_laws>
<section name="MISC">
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 18.028 * adc)"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<!-- Basic navigation settings -->
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
<define name="NAV_DESCEND_VSPEED" value="-0.7"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<!-- Settings for circle -->
<define name="DEFAULT_CIRCLE_RADIUS" value="700"/>
<define name="NAV_CARROT_DIST" value="200"/>
<!-- Avoid GPS loss behavior when having RC or datalink -->
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
</section>
<section name="FORWARD">
<!--The Nederdrone uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- This is the pitch angle that the Nederdrone will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/>
<!-- For RC coordinated turns, lower because the yawing was too slow -->
<define name="MAX_FWD_SPEED" value="20.0"/>
<!-- For hybrid guidance -->
<define name="MAX_AIRSPEED" value="20.0"/>
<!-- Enable airspeed measurements -->
<define name="USE_AIRSPEED" value="TRUE"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Rotate the IMU -->
<define name="MPU_CHAN_X" value="0"/>
<define name="MPU_CHAN_Y" value="2"/>
<define name="MPU_CHAN_Z" value="1"/>
<define name="MPU_X_SIGN" value="-1"/>
<define name="MPU_Y_SIGN" value="1"/>
<define name="MPU_Z_SIGN" value="1"/>
<!-- Calibrated with body only at cyberzoo (2022-08-22) -->
<define name="ACCEL_X_NEUTRAL" value="-92"/>
<define name="ACCEL_Y_NEUTRAL" value="11"/>
<define name="ACCEL_Z_NEUTRAL" value="-233"/>
<define name="ACCEL_X_SENS" value="4.893773666824744" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.830763157841358" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.700942730229992" integer="16"/>
<!-- NOT CALIBRATED -->
<define name="MAG_X_NEUTRAL" value="-2351"/>
<define name="MAG_Y_NEUTRAL" value="2060"/>
<define name="MAG_Z_NEUTRAL" value="4214"/>
<define name="MAG_X_SENS" value="0.6016027350789249" integer="16"/>
<define name="MAG_Y_SENS" value="0.6409447896809923" integer="16"/>
<define name="MAG_Z_SENS" value="0.648324840639345" integer="16"/>
<!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="77.7" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="80." unit="deg"/>
<define name="SP_MAX_R" value="180." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="60" unit="deg"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<define name="G1_ROLL" value="{ 0.8, -0.8, 1.6, -1.6, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 0.95, 0.95, -1.425, -1.425, -0.5, -0.5, -0.5, -0.5}"/>
<define name="G1_YAW" value="{ 0.00, 0.0, -0.00, 0.00, 0.4, -0.4, -0.4, 0.4}"/>
<define name="G1_THRUST" value="{ -0.3, -0.3, -0.45, -0.45, 0.0, 0.0, 0.0, 0.0}"/>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{0, 0, 0, 0, 0, 0, 0, 0}"/>
<!-- Forward gains -->
<define name="G1_ROLL_FWD" value="{ 0.5, -0.5, 0.96, -0.96, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH_FWD" value="{ 0.6, 0.6, -0.86, -0.86, -1.4, -1.4, -1.4, -1.4}"/>
<define name="G1_YAW_FWD" value="{ 0.0, 0.0, 0.0, 0.0, 0.94, -0.94, -0.94, 0.94}"/>
<define name="G1_THRUST_FWD" value="{-0.18 -0.18, -0.27, -0.27, 0.0, 0.0, 0.0, 0.0}"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="30.0"/>
<define name="REF_ERR_R" value="20.0"/>
<define name="REF_RATE_P" value="6.0"/>
<define name="REF_RATE_Q" value="6.0"/>
<define name="REF_RATE_R" value="6.0"/>
<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="180.0" unit="deg/s"/>
<!-- Maximum rate setpoint in rate control mode -->
<define name="MAX_RATE" value="3.0" unit="rad/s"/>
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="1.5"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
<define name="FILT_CUTOFF_P" value="20."/>
<define name="FILT_CUTOFF_Q" value="20."/>
<define name="FILT_CUTOFF_R" value="4."/>
<!-- first order actuator dynamics -->
<!--define name="ACT_DYN" value="{0.009, 0.009, 0.009, 0.009, 0.05, 0.05, 0.05, 0.05}"/-->
<define name="ACT_DYN" value="{0.028, 0.028, 0.028, 0.028, 0.05, 0.05, 0.05, 0.05}"/>
<define name="ACT_RATE_LIMIT" value="{9600, 9600, 9600, 9600, 170, 170, 170, 170}"/>
<define name="ACT_IS_SERVO" value="{0, 0, 0, 0, 1, 1, 1, 1}"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="310"/>
<define name="HOVER_KD" value="130"/>
<define name="HOVER_KI" value="10"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.42"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="28" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="60"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="0"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="a0,a1,a2,a3,a4,a5,a6,a7,a8" type="string[]"/>
<define name="JSBSIM_MODEL" value="nederdrone4_tem" type="string"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="COMMANDS_NB" value="9"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
<define name="DEBUG_SPEED_SP" value="false"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
</airframe>
conf/airframes/tudelft/nederdrone8.xml
+396
View File
@@ -0,0 +1,396 @@
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!-- This is a Nedderdrone with Trailing edge motors
* Airframe: TUD00???
* Autopilot: Pixhawk 4
* Actuators: 12x T-Motor ESC + Motors and 8x Servos (all CAN)
* Datalink: Herelink
* GPS: UBlox F9P
* RC: SBUS Crossfire
-->
<airframe name="Neddrone8">
<description>Neddrone8</description>
<firmware name="rotorcraft">
<target name="ap" board="px4fmu_5.0_chibios">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<configure name="FLASH_MODE" value="SWD"/>
<define name="USE_BARO_BOARD" value="1"/>
<module name="radio_control" type="sbus">
<configure name="SBUS_PORT" value="UART3"/>
</module>
<!-- Forward FuelCell data back to the GCS -->
<!--module name="generic_uart_sensor"/-->
<!-- Logger -->
<module name="tlsf"/>
<module name="pprzlog"/>
<module name="logger" type="sd_chibios"/>
<module name="flight_recorder"/>
<!-- Airspeed sensors -->
<module name="airspeed" type="ms45xx_i2c">
<define name="USE_I2C4"/>
<define name="MS45XX_I2C_DEV" value="i2c4"/>
</module>
<!--module name="airspeed" type="uavcan"/-->
<!-- Monitoring -->
<module name="sys_mon"/>
<!--module name="status_nederdrone"/-->
<define name="ADC_CURRENT_DISABLE" value="TRUE"/>
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="RADIO_AUX2"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<!-- <module name="ahrs" type="int_cmpl_quat">
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
<define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
</module> -->
<!--module name="ins" type="extended">
<define name="INS_USE_GPS_ALT" value="1"/>
<define name="INS_USE_GPS_ALT_SPEED" value="1"/>
<define name="INS_VFF_R_GPS" value="0.01"/>
</module-->
</target>
<target name="nps" board="pc">
<module name="radio_control" type="datalink"/>
<module name="fdm" type="jsbsim"/>
<module name="logger_file">
<define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
</module>
<!--Not dealing with these in the simulation-->
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control -->
<define name="RADIO_KILL_SWITCH" value="0"/>
</target>
<module name="eff_scheduling_nederdrone"/>
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/>
</module>
<module name="approach_moving_target">
<define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
</module>
<module name="ins" type="ekf2" />
<module name="actuators" type="uavcan">
<configure name="UAVCAN_USE_CAN1" value="TRUE"/>
<configure name="UAVCAN_USE_CAN2" value="TRUE"/>
</module>
<module name="imu" type="mpu6000"/>
<!--module name="gps" type="datalink"/-->
<module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module>
<module name="stabilization" type="indi">
<configure name="INDI_NUM_ACT" value="8"/>
<define name="TILT_TWIST_CTRL" value="TRUE"/>
</module>
<module name="stabilization" type="rate_indi"/>
<module name="air_data">
<define name="AIR_DATA_BARO_DIFF_ID" value="MS45XX_SENDER_ID"/> <!-- UAVCAN_SENDER_ID -->
<define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE" />
</module>
<!-- Internal MAG -->
<!--module name="mag_ist8310">
<define name="MODULE_IST8310_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_IST8310_I2C_DEV" value="I2C3"/>
</module-->
<!-- External MAG on GPS -->
<module name="mag_lis3mdl">
<define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
<define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
<define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
</module>
<!--module name="lidar" type="tfmini">
<configure name="TFMINI_PORT" value="UART4"/>
<configure name="USE_TFMINI_AGL" value="FALSE"/>
</module-->
<module name="guidance" type="indi_hybrid">
<define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
<define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.3"/>
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="0.5"/>
<define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
<define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
<define name="GUIDANCE_INDI_MAX_AIRSPEED" value="17.0"/>
<define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
<define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
<!--define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_45" value="-500.0"/>
<define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD" value="-1600.0"/-->
<!-- <define name="GUIDANCE_INDI_FILTER_CUTOFF" value="0.5"/> -->
<define name="GUIDANCE_INDI_LINE_GAIN" value="0.2"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE" value="1500"/>
<define name="GUIDANCE_INDI_MIN_THROTTLE_FWD" value="1500"/>
<define name="GUIDANCE_INDI_LIFTD_P50" value="6.0"/>
<define name="GUIDANCE_INDI_LIFTD_P80" value="10.0"/>
<define name="GUIDANCE_INDI_LIFTD_ASQ" value="0.15"/>
</module>
<module name="motor_mixing"/>
</firmware>
<!-- CAN BUS 1 (Front Wing) -->
<servos driver="Uavcan1">
<servo name="MOTOR_1" no="0" min="-8191" neutral="1500" max="8191"/>
<servo name="MOTOR_2" no="1" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_3" no="2" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_4" no="3" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_5" no="4" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_6" no="5" min="-8191" neutral="1500" max="8191"/>
<servo name="AIL_1" no="6" min="-6000" neutral="0" max="6000"/>
<servo name="AIL_2" no="7" min="6000" neutral="0" max="-6000"/>
</servos>
<!-- CAN BUS 2 (Back Wing) -->
<servos driver="Uavcan2">
<servo name="MOTOR_7" no="0" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_8" no="1" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_9" no="2" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_10" no="3" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_11" no="4" min="0" neutral="600" max="8191"/>
<servo name="MOTOR_12" no="5" min="0" neutral="600" max="8191"/>
<servo name="AIL_3" no="6" min="-6000" neutral="0" max="6000"/>
<servo name="AIL_4" no="7" min="6000" neutral="0" max="-6000"/>
</servos>
<commands>
<axis name="ROLL" failsafe_value="0"/>
<axis name="PITCH" failsafe_value="-300"/>
<axis name="YAW" failsafe_value="0"/>
<axis name="THRUST" failsafe_value="0"/>
</commands>
<rc_commands>
<set command="THRUST" value="@THROTTLE" />
<set command="ROLL" value="@YAW" />
<set command="PITCH" value="@PITCH/2" />
<set command="YAW" value="-@ROLL/4" />
</rc_commands>
<section name="MIXING" prefix="MOTOR_MIXING_">
<!-- frontleft left (CCW), frontleft mid (CW), frontleft right (CCW), frontright left (CW), frontright mid (CCW), frontright right (CW) -->
<!-- backleft left (CW), backleft mid (CCW), backleft right (CW), backright left (CCW), backright mid (CW), backright right (CCW) -->
<define name="NB_MOTOR" value="12"/>
<define name="SCALE" value="256"/>
<define name="ROLL_COEF" value="{256, 157, 56, -56, -157, -256, 256, 157, 56, -56, -157, -256}"/>
<define name="PITCH_COEF" value="{256, 256, 256, 256, 256, 256, -256, -256, -256, -256, -256, -256}"/>
<define name="YAW_COEF" value="{251, -256, 252, -252, 256, -251, -256, 252, -254, 254, -252, 256}"/>
<!--<define name="YAW_COEF" value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
<define name="THRUST_COEF" value="{256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256, 256}"/>
</section>
<command_laws>
<let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<!-- Tip props always at 80% -->
<call fun="actuators_pprz[8] = (Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*70*sched_ratio_tip_props);"/>
<set servo="MOTOR_1" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_2" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_3" value="($th_hold? -9600 : actuators_pprz[0])"/>
<set servo="MOTOR_4" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_5" value="($th_hold? -9600 : actuators_pprz[1])"/>
<set servo="MOTOR_6" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="MOTOR_7" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_8" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_9" value="($th_hold? -9600 : actuators_pprz[2])"/>
<set servo="MOTOR_10" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_11" value="($th_hold? -9600 : actuators_pprz[3])"/>
<set servo="MOTOR_12" value="($th_hold? -9600 : actuators_pprz[3])"/>
<!-- Removed ApplyDiff for differential control -->
<set servo="AIL_1" value="($th_hold? 9600 : actuators_pprz[4])"/>
<set servo="AIL_2" value="($th_hold? 9600 : actuators_pprz[5])"/>
<set servo="AIL_3" value="actuators_pprz[6]"/>
<set servo="AIL_4" value="actuators_pprz[7]"/>
</command_laws>
<section name="MISC">
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 17.8574 * adc)"/>
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<!-- Basic navigation settings -->
<define name="NAV_CLIMB_VSPEED" value="3.5"/>
<define name="NAV_DESCEND_VSPEED" value="-0.7"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<!-- Settings for circle -->
<define name="DEFAULT_CIRCLE_RADIUS" value="700"/>
<define name="NAV_CARROT_DIST" value="200"/>
<!-- Avoid GPS loss behavior when having RC or datalink -->
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
</section>
<section name="FORWARD">
<!--The Nederdrone uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
<define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
<!-- This is the pitch angle that the Nederdrone will have in forward flight, where 0 degrees is hover-->
<define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/>
<!-- For RC coordinated turns, lower because the yawing was too slow -->
<define name="MAX_FWD_SPEED" value="20.0"/>
<!-- For hybrid guidance -->
<define name="MAX_AIRSPEED" value="20.0"/>
<!-- Enable airspeed measurements -->
<define name="USE_AIRSPEED" value="TRUE"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- Rotate the IMU -->
<define name="MPU_CHAN_X" value="0"/>
<define name="MPU_CHAN_Y" value="2"/>
<define name="MPU_CHAN_Z" value="1"/>
<define name="MPU_X_SIGN" value="-1"/>
<define name="MPU_Y_SIGN" value="1"/>
<define name="MPU_Z_SIGN" value="1"/>
<!-- Calibated 2022-08-30 (Next to cyberzoo body only) -->
<define name="ACCEL_X_NEUTRAL" value="-40"/>
<define name="ACCEL_Y_NEUTRAL" value="45"/>
<define name="ACCEL_Z_NEUTRAL" value="-34"/>
<define name="ACCEL_X_SENS" value="4.892994928744868" integer="16"/>
<define name="ACCEL_Y_SENS" value="4.83273312391633" integer="16"/>
<define name="ACCEL_Z_SENS" value="4.92376963825951" integer="16"/>
<!-- Calibrated 2022-08-31 (Outside TU Delft) -->
<define name="MAG_X_NEUTRAL" value="69"/>
<define name="MAG_Y_NEUTRAL" value="2352"/>
<define name="MAG_Z_NEUTRAL" value="3381"/>
<define name="MAG_X_SENS" value="0.611984316047597" integer="16"/>
<define name="MAG_Y_SENS" value="0.6321285137117578" integer="16"/>
<define name="MAG_Z_SENS" value="0.6495870967651789" integer="16"/>
<!-- Define axis in hover frame -->
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="75.3" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
<!-- setpoints -->
<define name="SP_MAX_PHI" value="60." unit="deg"/>
<define name="SP_MAX_THETA" value="80." unit="deg"/>
<define name="SP_MAX_R" value="180." unit="deg/s"/>
<define name="DEADBAND_R" value="200"/>
<define name="SP_PSI_DELTA_LIMIT" value="60" unit="deg"/>
</section>
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
<define name="G1_ROLL" value="{ 0.8, -0.8, 1.6, -1.6, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH" value="{ 0.95, 0.95, -1.425, -1.425, -0.5, -0.5, -0.5, -0.5}"/>
<define name="G1_YAW" value="{ 0.00, 0.0, -0.00, 0.00, 0.4, -0.4, -0.4, 0.4}"/>
<define name="G1_THRUST" value="{ -0.3, -0.3, -0.45, -0.45, 0.0, 0.0, 0.0, 0.0}"/>
<!--Counter torque effect of spinning up a rotor-->
<define name="G2" value="{0, 0, 0, 0, 0, 0, 0, 0}"/>
<!-- Forward gains -->
<define name="G1_ROLL_FWD" value="{ 0.5, -0.5, 0.96, -0.96, 0.0, 0.0, 0.0, 0.0}"/>
<define name="G1_PITCH_FWD" value="{ 0.6, 0.6, -0.86, -0.86, -1.4, -1.4, -1.4, -1.4}"/>
<define name="G1_YAW_FWD" value="{ 0.0, 0.0, 0.0, 0.0, 0.94, -0.94, -0.94, 0.94}"/>
<define name="G1_THRUST_FWD" value="{-0.3 -0.3, -0.45, -0.45, 0.0, 0.0, 0.0, 0.0}"/>
<!-- reference acceleration for attitude control -->
<define name="REF_ERR_P" value="30.0"/>
<define name="REF_ERR_Q" value="30.0"/>
<define name="REF_ERR_R" value="20.0"/>
<define name="REF_RATE_P" value="6.0"/>
<define name="REF_RATE_Q" value="6.0"/>
<define name="REF_RATE_R" value="6.0"/>
<!--Maxium yaw rate, to avoid instability-->
<define name="MAX_R" value="180.0" unit="deg/s"/>
<!-- Maximum rate setpoint in rate control mode -->
<define name="MAX_RATE" value="3.0" unit="rad/s"/>
<!-- second order filter parameters -->
<define name="FILT_CUTOFF" value="1.5"/>
<define name="FILT_CUTOFF_RDOT" value="0.5"/>
<define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
<define name="FILT_CUTOFF_P" value="20."/>
<define name="FILT_CUTOFF_Q" value="20."/>
<define name="FILT_CUTOFF_R" value="4."/>
<!-- first order actuator dynamics -->
<!--define name="ACT_DYN" value="{0.009, 0.009, 0.009, 0.009, 0.05, 0.05, 0.05, 0.05}"/-->
<define name="ACT_DYN" value="{0.028, 0.028, 0.028, 0.028, 0.05, 0.05, 0.05, 0.05}"/>
<define name="ACT_RATE_LIMIT" value="{9600, 9600, 9600, 9600, 170, 170, 170, 170}"/>
<define name="ACT_IS_SERVO" value="{0, 0, 0, 0, 1, 1, 1, 1}"/>
<!-- Adaptive Learning Rate -->
<define name="USE_ADAPTIVE" value="FALSE"/>
<define name="ADAPTIVE_MU" value="0.0001"/>
<!-- Trims -->
<define name="TRIM_ELEVATOR" value="1800"/>
</section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<define name="HOVER_KP" value="310"/>
<define name="HOVER_KD" value="130"/>
<define name="HOVER_KI" value="10"/>
<define name="NOMINAL_HOVER_THROTTLE" value="0.42"/>
<define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
</section>
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
<define name="MAX_BANK" value="28" unit="deg"/>
<define name="USE_SPEED_REF" value="TRUE"/>
<define name="PGAIN" value="60"/>
<define name="DGAIN" value="100"/>
<define name="AGAIN" value="0"/>
<define name="IGAIN" value="20"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="a0,a1,a2,a3,a4,a5,a6,a7,a8" type="string[]"/>
<define name="JSBSIM_MODEL" value="nederdrone4_tem" type="string"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="COMMANDS_NB" value="9"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
<define name="JS_AXIS_MODE" value="4"/>
<define name="DEBUG_SPEED_SP" value="false"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_NAV"/>
</section>
<section name="BAT">
<define name="CATASTROPHIC_BAT_LEVEL" value="18.0" unit="V"/>
<define name="CRITIC_BAT_LEVEL" value="18.6" unit="V"/>
<define name="LOW_BAT_LEVEL" value="19.2" unit="V"/>
<define name="MAX_BAT_LEVEL" value="25.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
</airframe>
conf/conf_tests.xml
+1 -1
View File
@@ -7,7 +7,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
/>
<aircraft
conf/flight_plans/tudelft/nederdrone_terschelling.xml
+155
View File
@@ -0,0 +1,155 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="60" ground_alt="0" lat0="53.398848" lon0="5.258644" max_dist_from_home="5000" name="Nederdrone Valkenburg" security_height="2">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
</header>
<waypoints>
<waypoint name="HOME" x="-657.7" y="-25.2"/>
<waypoint name="CLIMB" x="-476.9" y="77.1"/>
<waypoint alt="80.0" name="STDBY" x="-688.0" y="13.9"/>
<waypoint alt="100.0" name="p1" x="-999.0" y="87.9"/>
<waypoint alt="100.0" name="p2" x="-1140.3" y="367.8"/>
<waypoint alt="100.0" name="p3" x="-734.2" y="511.2"/>
<waypoint alt="100.0" name="p4" x="-616.5" y="237.7"/>
<waypoint name="TD" x="-472.8" y="169.5"/>
<waypoint name="FOLLOW" x="-593.9" y="379.7"/>
<waypoint lat="53.409169" lon="5.243295" name="S1"/>
<waypoint lat="53.410073" lon="5.248888" name="S2"/>
<waypoint lat="53.403856" lon="5.222350" name="C1"/>
<waypoint lat="53.399328" lon="5.226276" name="C2"/>
<waypoint lat="53.391885" lon="5.237441" name="C3"/>
<waypoint lat="53.399786" lon="5.273960" name="C4"/>
<waypoint lat="53.413211" lon="5.265218" name="C5"/>
<waypoint lat="53.421785" lon="5.253991" name="C6"/>
<waypoint lat="53.418792" lon="5.235271" name="C7"/>
<waypoint lat="53.415670" lon="5.219705" name="C8"/>
<waypoint lat="53.411297" lon="5.216826" name="C9"/>
</waypoints>
<sectors>
<sector color="red" name="Flyzone">
<corner name="C1"/>
<corner name="C2"/>
<corner name="C3"/>
<corner name="C4"/>
<corner name="C5"/>
<corner name="C6"/>
<corner name="C7"/>
<corner name="C8"/>
<corner name="C9"/>
</sector>
</sectors>
<modules>
<module name="nav" type="survey_rectangle_rotorcraft">
<define name="RECTANGLE_SURVEY_DEFAULT_SWEEP" value="100"/>
</module>
</modules>
<exceptions>
<exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine">
<set value="FALSE" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<call_once fun="nav_set_heading_current()"/>
<attitude pitch="-60" roll="0" throttle="0" until="stage_time>1" vmode="throttle"/>
</block>
<block name="TakeoffLow" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="GetPosHeight() @GT 18.0" deroute="Takeoff"/>
<call_once fun="nav_set_heading_current()"/>
<attitude pitch="-40." roll="0" throttle="0.8" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="GetPosHeight() @GT 40.0" deroute="Standby"/>
<call_once fun="nav_set_heading_current()"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="FALSE" var="force_forward"/>
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<set value="FALSE" var="force_forward"/>
<stay wp="p1"/>
</block>
<block name="go_p2">
<set value="TRUE" var="force_forward"/>
<call_once fun="nav_set_heading_deg(90)"/>
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="line_p1_p2">
<set value="TRUE" var="force_forward"/>
<go from="p1" hmode="route" wp="p2"/>
<stay until="stage_time>10" wp="p2"/>
<go from="p2" hmode="route" wp="p1"/>
<deroute block="line_p1_p2"/>
</block>
<block name="route">
<set value="TRUE" var="force_forward"/>
<go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p3"/>
<go from="p3" hmode="route" wp="p4"/>
<go from="p4" hmode="route" wp="p1"/>
<deroute block="route"/>
</block>
<block group="extra_pattern" name="Survey S1-S2 25m NS" strip_button="Svy25-NS-PRIMITIVE">
<set value="FALSE" var="force_forward"/>
<survey_rectangle grid="30" orientation="NS" wp1="S1" wp2="S2"/>
</block>
<block group="extra_pattern" name="Survey S1-S2 NS" strip_button="Svy-NS" strip_icon="survey.png">
<set value="TRUE" var="force_forward"/>
<call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S2, sweep, NS)"/>
<deroute block="Survey RECTANGLE RUN"/>
</block>
<block group="extra_pattern" name="Survey S1-S2 LO" strip_button="Svy-LO" strip_icon="survey_we.png">
<set value="TRUE" var="force_forward"/>
<call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S2, sweep, WE)"/>
<deroute block="Survey RECTANGLE RUN"/>
</block>
<block group="extra_pattern" name="Survey RECTANGLE RUN" strip_button="Svy CONT">
<set value="TRUE" var="force_forward"/>
<exception cond="rectangle_survey_sweep_num @GT 1" deroute="Standby"/>
<call fun="nav_survey_rectangle_rotorcraft_run(WP_S1, WP_S2)"/>
</block>
<block name="Oval">
<set value="TRUE" var="force_forward"/>
<oval p1="p1" p2="p2" radius="nav_radius"/>
</block>
<block name="land here" strip_button="Land Here" strip_icon="land-right.png">
<set value="FALSE" var="force_forward"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land">
<set value="FALSE" var="force_forward"/>
<go wp="TD"/>
</block>
<block name="descend">
<set value="FALSE" var="force_forward"/>
<exception cond="LessThan(GetPosHeight(), 15.0)" deroute="flare"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<set value="FALSE" var="force_forward"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/flight_plans/tudelft/nederdrone_troia.xml
+220
View File
@@ -0,0 +1,220 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="100" ground_alt="0" lat0="38.474472222222225" lon0="-8.871222222222222" max_dist_from_home="80000" name="Nederdrone Troia" security_height="2">
<header>
#include "autopilot.h"
#include "modules/datalink/datalink.h"
</header>
<waypoints>
<waypoint lat="38.474472222222225" lon="-8.871222222222222" name="HOME"/>
<waypoint lat="38.476515" lon="-8.868380" name="CLIMB"/>
<waypoint lat="38.476331" lon="-8.869787" name="STDBY"/>
<waypoint lat="38.476940" lon="-8.867501" name="p1"/>
<waypoint lat="38.481886" lon="-8.856215" name="p2"/>
<!-- <waypoint lat="38.478809" lon="-8.851799" name="p3"/> -->
<!-- <waypoint lat="38.480182" lon="-8.853301" name="p4"/> -->
<waypoint lat="38.475523" lon="-8.867495" name="s1"/>
<waypoint lat="38.478711" lon="-8.860859" name="s2"/>
<waypoint lat="38.476479" lon="-8.859367" name="s3"/>
<waypoint lat="38.473650" lon="-8.865674" name="s4"/>
<waypoint lat="38.480073" lon="-8.872210" name="n1"/>
<waypoint lat="38.483435" lon="-8.865682" name="n2"/>
<waypoint lat="38.480763" lon="-8.863378" name="n3"/>
<waypoint lat="38.477503" lon="-8.869605" name="n4"/>
<waypoint lat="38.475002" lon="-8.870565" name="TD"/>
<waypoint lat="38.474497" lon="-8.870086" name="APP"/>
<waypoint lat="38.499227" lon="-8.858617" name="1-1"/>
<waypoint lat="38.476154" lon="-8.833249" name="1-2"/>
<waypoint lat="38.469040" lon="-8.861433" name="1-3"/>
<waypoint lat="38.467811" lon="-8.879136" name="1-4"/>
<waypoint lat="38.464900" lon="-8.884918" name="1-5"/>
<waypoint lat="38.471784" lon="-8.899161" name="1-6"/>
<waypoint lat="38.478446" lon="-8.885780" name="1-7"/>
<waypoint lat="38.480130" lon="-8.886261" name="1-8"/>
<waypoint lat="38.467526" lon="-8.888318" name="_2-1"/>
<waypoint lat="38.466563" lon="-8.887256" name="_2-2"/>
<waypoint lat="38.470725" lon="-8.881204" name="_2-4"/>
<waypoint lat="38.469633" lon="-8.880184" name="_2-3"/>
<waypoint lat="38.493038" lon="-8.858636" name="1-1_soft"/>
<waypoint lat="38.478620" lon="-8.845008" name="1-2_soft"/>
<waypoint lat="38.470562" lon="-8.867834" name="1-3_soft"/>
<waypoint lat="38.476390" lon="-8.887219" name="1-5_soft"/>
<waypoint lat="38.468803" lon="-8.880064" name="1-4_soft"/>
<waypoint lat="38.468493" lon="-8.886115" name="2-1_soft"/>
<waypoint lat="38.467498" lon="-8.884860" name="2-2_soft"/>
<waypoint lat="38.475621" lon="-8.874463" name="2-4_soft"/>
<waypoint lat="38.468610" lon="-8.877984" name="2-3_soft"/>
<waypoint lat="38.4" lon="-9.608888888888888" name="6-1"/>
<waypoint lat="38.409166666666664" lon="-9.196944444444444" name="6-2"/>
<waypoint lat="38.434444444444445" lon="-9.116666666666667" name="6-3"/>
<waypoint lat="38.43361111111111" lon="-9.054722222222223" name="6-4"/>
<waypoint lat="38.48777777777778" lon="-8.93361111111111" name="6-5"/>
<waypoint lat="38.46722222222222" lon="-8.885555555555555" name="6-6"/>
<waypoint lat="38.44444444444444" lon="-8.854722222222222" name="6-7"/>
<waypoint lat="38.30027777777778" lon="-8.854722222222222" name="6-8"/>
<waypoint lat="38.30027777777778" lon="-8.850833333333334" name="6-9"/>
<waypoint lat="38.215833333333336" lon="-8.850277777777778" name="6-10"/>
<waypoint lat="38.16833333333334" lon="-9.001666666666667" name="6-11"/>
<waypoint lat="38.16694444444445" lon="-9.198888888888888" name="6-12"/>
<waypoint lat="38.000277777777775" lon="-9.200555555555555" name="6-13"/>
<waypoint lat="38.000277777777775" lon="-9.450277777777778" name="6-14"/>
</waypoints>
<sectors>
<sector color="red" name="A1">
<corner name="1-1"/>
<corner name="1-2"/>
<corner name="1-3"/>
<corner name="1-4"/>
<corner name="1-5"/>
<corner name="1-6"/>
<corner name="1-7"/>
<corner name="1-8"/>
</sector>
<sector color="orange" name="A1_soft">
<corner name="1-1_soft"/>
<corner name="1-2_soft"/>
<corner name="1-3_soft"/>
<corner name="1-4_soft"/>
<corner name="1-5_soft"/>
</sector>
<sector color="red" name="Corridor">
<corner name="_2-1"/>
<corner name="_2-2"/>
<corner name="_2-3"/>
<corner name="_2-4"/>
</sector>
<sector color="red" name="A3">
<corner name="6-1"/>
<corner name="6-2"/>
<corner name="6-3"/>
<corner name="6-4"/>
<corner name="6-5"/>
<corner name="6-6"/>
<corner name="6-7"/>
<corner name="6-8"/>
<corner name="6-9"/>
<corner name="6-10"/>
<corner name="6-11"/>
<corner name="6-12"/>
<corner name="6-13"/>
<corner name="6-14"/>
</sector>
</sectors>
<modules>
<module name="nav" type="survey_rectangle_rotorcraft">
<define name="RECTANGLE_SURVEY_DEFAULT_SWEEP" value="100"/>
</module>
</modules>
<exceptions>
<!-- GPS Loss (8 seconds -> gps_loss) -->
<exception cond="!GpsFixValid() @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('gps_lost'))" deroute="gps_lost"/>
<!-- Hard geofence (red -> Holding point) -->
<exception cond="And( Or(!InsideA1(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 152.4),
Or(!InsideA3(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 304.8))
@AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
<!-- Soft geofence (orange -> GO NORTH) -->
<exception cond="And( Or(!InsideA1_soft(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 152.4),
Or(!InsideA3(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 304.8))
@AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('GO NORTH'))" deroute="GO NORTH"/>
<!-- Datalink loss (25 seconds -> GO NORTH) -->
<exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('GO NORTH'))" deroute="GO NORTH"/>
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init">
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point">
<call_once fun="NavKillThrottle()"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Takeoff" strip_icon="takeoff.png">
<set value="FALSE" var="force_forward"/>
<call_once fun="NavResurrect()"/>
<call_once fun="nav_set_heading_current()"/>
<attitude pitch="-50" roll="0" throttle="0.8" until="stage_time>0" vmode="throttle"/>
</block>
<block name="TakeoffLow">
<exception cond="GetPosHeight() @GT 18.0" deroute="Takeoff"/>
<call_once fun="nav_set_heading_current()"/>
<attitude pitch="-45." roll="0" throttle="0.9" vmode="throttle"/>
</block>
<block name="Takeoff">
<exception cond="GetPosHeight() @GT 70.0" deroute="GO NORTH"/>
<call_once fun="nav_set_heading_current()"/>
<call_once fun="waypoint_set_xy_i(WP_CLIMB,POS_BFP_OF_REAL(GetPosX()+20.0*sinf(stateGetNedToBodyEulers_f()@DEREFpsi)),POS_BFP_OF_REAL(GetPosY()+20.0*cosf(stateGetNedToBodyEulers_f()@DEREFpsi)))"/>
<stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="GO SOUTH">
<set value="TRUE" var="force_forward"/>
<go from="s1" hmode="route" wp="s2"/>
<go from="s2" hmode="route" wp="s3"/>
<go from="s3" hmode="route" wp="s4"/>
<go from="s4" hmode="route" wp="s1"/>
<deroute block="GO SOUTH"/>
</block>
<block name="GO NORTH">
<set value="TRUE" var="force_forward"/>
<go from="n1" hmode="route" wp="n2"/>
<go from="n2" hmode="route" wp="n3"/>
<go from="n3" hmode="route" wp="n4"/>
<go from="n4" hmode="route" wp="n1"/>
<deroute block="GO NORTH"/>
</block>
<block name="Approach APP">
<stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="home.png">
<set value="FALSE" var="force_forward"/>
<stay wp="STDBY"/>
</block>
<block name="stay_p1">
<set value="FALSE" var="force_forward"/>
<stay wp="p1"/>
</block>
<block name="go_p2">
<set value="TRUE" var="force_forward"/>
<call_once fun="nav_set_heading_deg(90)"/>
<go wp="p2"/>
<deroute block="stay_p1"/>
</block>
<block name="line_p1_p2">
<set value="TRUE" var="force_forward"/>
<go from="p1" hmode="route" wp="p2"/>
<go from="p2" hmode="route" wp="p1"/>
<deroute block="line_p1_p2"/>
</block>
<block name="gps_lost">
<exception cond="GpsFixValid()" deroute="GO NORTH"/>
<attitude pitch="0" roll="0" climb="-1.0" vmode="climb"/>
</block>
<block name="Oval">
<set value="TRUE" var="force_forward"/>
<oval p1="p1" p2="p2" radius="nav_radius"/>
</block>
<block name="land here">
<set value="FALSE" var="force_forward"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land" strip_button="Land Here" strip_icon="land-right.png">
<set value="FALSE" var="force_forward"/>
<go wp="TD"/>
</block>
<block name="descend">
<set value="FALSE" var="force_forward"/>
<exception cond="LessThan(GetPosHeight(), 15.0)" deroute="flare"/>
<stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="flare">
<set value="FALSE" var="force_forward"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
<block name="landed">
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/modules/approach_moving_target.xml
+34
View File
@@ -0,0 +1,34 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="approach_moving_target" dir="ctrl">
<doc>
<description>
Approach a moving target (e.g. ship) along a diagonal.
</description>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="Approach">
<dl_setting var="amt.psi_ref" min="-180." max="180." step="1.0" shortname="psi_app" unit="degrees"/>
<dl_setting var="amt.slope_ref" min="0." max="80." step="1.0" shortname="slope_app" unit="degrees"/>
<dl_setting var="amt.speed" min="-5.0" max="5." step="0.1" shortname="speed" unit="m/s"/>
<dl_setting var="amt.distance" min="0.0" max="200." step="1.0" shortname="dist" unit="m"/>
<dl_setting var="amt_err_slowdown_gain" min="0.0" max="4." step="0.01" shortname="slwdn_gain"/>
<dl_setting var="amt.pos_gain" min="0.05" max="3.0" step="0.01" shortname="pos_gain"/>
<dl_setting var="amt.speed_gain" min="0.0" max="1.0" step="0.01" shortname="speed_gain"/>
<dl_setting var="amt.relvel_gain" min="0.0" max="1.0" step="0.01" shortname="relvel_gain"/>
</dl_settings>
</dl_settings>
</settings>
<dep>
<depends>target_pos</depends>
</dep>
<header>
<file name="approach_moving_target.h"/>
</header>
<init fun="approach_moving_target_init()"/>
<periodic fun="follow_diagonal_approach()" freq="100.0"/>
<makefile>
<file name="approach_moving_target.c"/>
</makefile>
</module>
conf/modules/eff_scheduling_nederdrone.xml
+30
View File
@@ -0,0 +1,30 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="eff_scheduling_nederdrone" dir="ctrl">
<doc>
<description>Interpolation of control effectivenss matrix
of the Nederdrone.
If instead using online adaptation is an option, be sure to
not use this module at the same time!</description>
</doc>
<settings>
<dl_settings>
<dl_settings name="eff_scheduling">
<dl_setting shortname="tip_prop_always_on" var="sched_tip_props_always_on" min="0" step="1" max="1"/>
<dl_setting shortname="tip_prop_lower_pitch" var="sched_tip_prop_lower_pitch_limit_deg" min="-90" step="1" max="0"/>
<dl_setting shortname="tip_prop_upper_pitch" var="sched_tip_prop_upper_pitch_limit_deg" min="-90" step="1" max="0"/>
<dl_setting shortname="sched_ratio_tip_props" var="sched_ratio_tip_props" min="0" step="0.1" max="1"/>
<dl_setting shortname="thrust_eff_scaling" var="thrust_eff_scaling" min="0.6" step="0.01" max="1.4"/>
<dl_setting shortname="all_act_fwd_sched" var="all_act_fwd_sched" min="0" step="1" max="1" values="OFF|ON"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="eff_scheduling_nederdrone.h"/>
</header>
<init fun="ctrl_eff_scheduling_init()"/>
<periodic fun="ctrl_eff_scheduling_periodic()" freq="20.0"/>
<makefile target="ap|nps" firmware="rotorcraft">
<file name="eff_scheduling_nederdrone.c"/>
</makefile>
</module>
conf/modules/target_pos.xml
+39
View File
@@ -0,0 +1,39 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="target_pos" dir="ctrl">
<doc>
<description>
Target position calculation
</description>
<section name="TARGET_POS" prefix="TARGET_">
<define name="POS_TIMEOUT" value="5000" description="timeout of the ground target position message in msec"/>
<define name="OFFSET_HEADING" value="180." description="target offset heading in degrees"/>
<define name="OFFSET_DISTANCE" value="10." description="target offset distance in meters"/>
<define name="OFFSET_HEIGHT" value="10." description="target offset height in meters"/>
<define name="TARGET_INTEGRATE_XY" value="true" description="enable target position xy (North/East) integration"/>
<define name="TARGET_INTEGRATE_Z" value="true" description="enable target position z (North/East) integration"/>
</section>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="TargetPos">
<dl_setting var="target.offset.distance" min="0." max="100." step="0.1" shortname="distance" unit="m"/>
<dl_setting var="target.offset.height" min="-10." max="200." step="0.1" shortname="height" unit="m"/>
<!-- Heading relative wrt. the ship!! -->
<dl_setting var="target.offset.heading" min="0." max="360." step="1.0" shortname="heading_wrt_ship"/>
<dl_setting var="target.integrate_xy" min="0" max="1" step="1" shortname="integrate_xy" values="OFF|ON"/>
<dl_setting var="target.target_pos_timeout" min="0" max="30000" step="100" shortname="target_pos_timeout"/>
<dl_setting var="target.rtk_timeout" min="0" max="30000" step="100" shortname="rtk_timeout"/>
<dl_setting var="target.offset.heading" min="0." max="360." step="1.0" type="float" shortname ="set_current" module="modules/ctrl/target_pos" handler="set_current_offset"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="target_pos.h"/>
</header>
<init fun="target_pos_init()"/>
<datalink message="TARGET_POS" fun="target_parse_target_pos(buf)"/>
<makefile>
<file name="target_pos.c"/>
</makefile>
</module>
conf/pprz_center_settings.ini
+4
View File
@@ -0,0 +1,4 @@
[ui]
last_AC=CubeOrange
last_session=Flight ACM1 Transparent @115200
window_size=@Size(1624 986)
conf/simulator/jsbsim/aircraft/nederdrone4_tem.xml
+578
View File
File diff suppressed because it is too large Load Diff
conf/userconf/tudelft/conf.xml
+70 -33
View File
@@ -9,6 +9,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/cv_opticflow.xml] modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
gui_color="red"
release="ac5eaa8da36b999a135f979e598930a9d40f6401"
/>
<aircraft
name="ARDrone2_default"
@@ -18,8 +19,8 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/video_rtp_stream.xml"
gui_color="#ffffd633d633"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/video_rtp_stream.xml"
/>
<aircraft
name="ARDrone2_indi"
@@ -119,6 +120,7 @@
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="#ffffbc3bce5b"
release="934f0e14c09da0a3bfe199b7685abd2f4b6badbe"
/>
<aircraft
name="CX10"
@@ -163,6 +165,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/heli_throttle_curve.xml modules/imu_common.xml modules/logger_sd_spi_direct.xml modules/nav_basic_rotorcraft.xml modules/opa_controller.xml modules/stabilization_int_quat.xml modules/stabilization_rate.xml modules/temp_adc.xml"
gui_color="#ffffdffac31f"
release="7c6064c5c3522e3286af8ecbc7498773b421ba7d"
/>
<aircraft
name="Disco"
@@ -174,6 +177,7 @@
settings="settings/fixedwing_basic.xml"
settings_modules="modules/ahrs_float_dcm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
gui_color="blue"
release="dfba3220f5b3500611231d02fca8799f9f045cb8"
/>
<aircraft
name="Disco_FlatSpin"
@@ -185,6 +189,7 @@
settings="settings/fixedwing_basic.xml"
settings_modules="modules/ahrs_float_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_adaptive_fw.xml"
gui_color="blue"
release="57fa5506ad03c85215b0ff7659d9f2a26131db1f"
/>
<aircraft
name="Flappy"
@@ -196,6 +201,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml"
gui_color="white"
release="65aa9a711dc4a66ed2d7cf73c0f5872bbeeb821d"
/>
<aircraft
name="Heli450"
@@ -218,6 +224,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/air_data.xml] modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="#ffffcccaccca"
release="433c2150022275f26fb4619e536903676d7e2c9e"
/>
<aircraft
name="LadyLisaBluegiga"
@@ -229,6 +236,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml"
gui_color="blue"
release="4f95168b07005474c8fedf3be5c2f2b4f4c7c393"
/>
<aircraft
name="LadybirdMXS"
@@ -251,6 +259,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/heli_throttle_curve.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml"
gui_color="#ffffdffac31f"
release="0fba1ce80a46d8dce23c570e6547595c89988fad"
/>
<aircraft
name="MAVTec1_Kirk"
@@ -262,6 +271,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="#ffff00000000"
release="e688107d8a1f9c933cb0931eebb0e7b009ccc445"
/>
<aircraft
name="Mentor"
@@ -273,6 +283,7 @@
settings="settings/fixedwing_basic.xml settings/estimation/ac_char.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_energy.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/stabilization_attitude_fw.xml"
gui_color="white"
release="997fa535902c4d8f73bc34c02c862652cd47cae5"
/>
<aircraft
name="Nederdrone4"
@@ -280,21 +291,47 @@
airframe="airframes/tudelft/nederdrone4.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
flight_plan="flight_plans/tudelft/nederdrone_troia.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/air_data.xml modules/approach_moving_target.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml modules/target_pos.xml"
gui_color="blue"
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
/>
<aircraft
name="Nederdrone5"
name="Nederdrone6"
ac_id="27"
airframe="airframes/tudelft/nederdrone5.xml"
airframe="airframes/tudelft/nederdrone6.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
flight_plan="flight_plans/tudelft/nederdrone_troia.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
gui_color="blue"
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
/>
<aircraft
name="Nederdrone7"
ac_id="2"
airframe="airframes/tudelft/nederdrone7.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/nederdrone_troia.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
gui_color="blue"
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
/>
<aircraft
name="Nederdrone8"
ac_id="5"
airframe="airframes/tudelft/nederdrone8.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/nederdrone_troia.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/target_pos.xml"
gui_color="#ffff00000000"
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
/>
<aircraft
name="Nederquad"
@@ -304,7 +341,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_basic.xml"
settings="[settings/rotorcraft_basic.xml] [settings/control/stabilization_rate.xml] [settings/control/stabilization_indi.xml]"
settings_modules="modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/lidar_lite.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_mateksys_3901_l0x.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/lidar_lite.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_mateksys_3901_l0x.xml modules/stabilization_indi_simple.xml"
gui_color="#406b937dccd0"
/>
<aircraft
@@ -317,6 +354,7 @@
settings="settings/rotorcraft_basic.xml settings/nps.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/range_forcefield.xml modules/stabilization_indi_simple.xml modules/stabilization_int_quat.xml"
gui_color="blue"
release="c11f888de1eba08d751f1ff0b03bf4a5bec8e8fa"
/>
<aircraft
name="Quadplane"
@@ -328,6 +366,7 @@
settings="settings/nps.xml settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/sys_id_chirp.xml"
gui_color="blue"
release="f739a81cfa2c633e33f31ab5f095d8f617276974"
/>
<aircraft
name="Robird"
@@ -339,6 +378,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/air_data.xml] modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="#ffffcccaccca"
release="677c4c77398cfbabffae3efdcafdacfd04a0b9ab"
/>
<aircraft
name="Splash"
@@ -350,6 +390,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/air_data.xml] modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/spektrum_soft_bind.xml modules/stabilization_indi_simple.xml"
gui_color="#fff0cccaccca"
release="234e898f7fd1483806808313d3086f12a1b5730a"
/>
<aircraft
name="Splash3"
@@ -361,6 +402,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/stabilization_rate.xml"
gui_color="blue"
release="009b121cfd12be8d3e236e0cbd5b564f2639d54f"
/>
<aircraft
name="Splash4"
@@ -373,6 +415,18 @@
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="blue"
/>
<aircraft
name="StaticTest_Ned8"
ac_id="19"
airframe="airframes/tudelft/nederdrone8_statictest.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/nederdrone_troia_big_airspace.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="[modules/air_data.xml] modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/eff_scheduling_nederdrone_dummy.xml modules/follow_me.xml modules/gps.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml [modules/imu_common.xml] modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_basic_rotorcraft.xml [modules/nav_survey_rectangle_rotorcraft.xml] modules/stabilization_indi.xml modules/target_pos.xml"
gui_color="#ffff00000000"
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
/>
<aircraft
name="TwoSeasTwenty"
ac_id="200"
@@ -383,6 +437,7 @@
settings="settings/fixedwing_basic.xml"
settings_modules="modules/gps.xml modules/guidance_basic_fw.xml modules/light.xml modules/nav_basic_fw.xml modules/stabilization_attitude_fw.xml"
gui_color="blue"
release="679fcad03bab95b2982cb6d8e3537486da45d306"
/>
<aircraft
name="Walkera_GeniusV1"
@@ -436,19 +491,9 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_bebop.xml"
settings="settings/rotorcraft_basic.xml [settings/estimation/ahrs_secondary.xml]"
settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="#baa3d698b729"
/>
<aircraft
name="bebop2_convergence"
ac_id="5"
airframe="airframes/tudelft/bebop2_indi_convergence.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/delft_bebop_convergence.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
release="cde043338785efbaa7aac97e90cb272099a0c7ec"
/>
<aircraft
name="bebop2_no_dampers"
@@ -508,24 +553,14 @@
<aircraft
name="bebop_opticflow_indoor"
ac_id="97"
airframe="airframes/tudelft/bebop_opticflow_indoor.xml"
airframe="airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml"
radio="radios/dummy.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotorcraft_cyberzoo_no_gps.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml modules/video_capture.xml modules/video_rtp_stream.xml"
gui_color="blue"
/>
<aircraft
name="disco_convergence"
ac_id="2"
airframe="airframes/tudelft/disco_rotorcraft_indi.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/disco_convergence.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/bebop_ae_awb.xml modules/bebop_cam.xml modules/cv_opticflow.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_hff_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml modules/video_capture.xml modules/video_rtp_stream.xml"
gui_color="blue"
release="398071ca8f7c8dc24becbe76bc5628b6ea604248"
/>
<aircraft
name="fan_demo"
@@ -537,6 +572,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml [modules/geo_mag.xml] modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="#ffffcccaccca"
release="f580c07814aa1068bf3f6960318b2417fc069cbe"
/>
<aircraft
name="frog1"
@@ -559,6 +595,7 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml modules/gps.xml modules/guidance_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="blue"
release="93a9608c3bc34eca6a0dcde4123d55dac7d7b448"
/>
<aircraft
name="quadshot_pylons"
sw/airborne/math/pprz_algebra_float.c
+14 -3
View File
@@ -620,11 +620,11 @@ void float_quat_of_rmat(struct FloatQuat *q, struct FloatRMat *rm)
/**
* @brief Tilt twist decomposition of a quaternion (z axis)
*
*
* Decomposes a quaternion rotation in two rotations:
* 1. A rotation that aligns the z axis.
* 2. A rotation around the z axis.
*
*
* Useful for control of vehicles that are slow in rotation around the z axis.
*
* @param tilt Tilt output
@@ -643,7 +643,7 @@ void float_quat_tilt_twist(struct FloatQuat *tilt, struct FloatQuat *twist, stru
// The cross product gives the axis around which to rotate to match the Z vectors.
struct FloatVect3 axis;
VECT3_CROSS_PRODUCT(axis, z, z_rot);
VECT3_CROSS_PRODUCT(axis, z, z_rot);
tilt->qi = 1 + z_rot.z;
tilt->qx = axis.x;
@@ -1021,6 +1021,17 @@ void vect_bound_in_2d(struct FloatVect3 *vect3, float bound) {
}
}
/* Scale a 3D vector to within a 3D bound */
void vect_bound_in_3d(struct FloatVect3 *vect3, float bound) {
float norm = FLOAT_VECT3_NORM(*vect3);
if(norm>bound) {
float scale = bound/norm;
vect3->x *= scale;
vect3->y *= scale;
vect3->z *= scale;
}
}
/* Scale a 3D vector to a certain length in 2D */
void vect_scale(struct FloatVect3 *vect3, float norm_des) {
float norm = FLOAT_VECT2_NORM(*vect3);
sw/airborne/math/pprz_algebra_float.h
+1
View File
@@ -886,6 +886,7 @@ extern void float_mat2_mult(struct FloatVect2 *vect_out, float mat[4], struct Fl
extern bool float_mat_inv_4d(float invOut[16], float mat_in[16]);
extern void vect_bound_in_2d(struct FloatVect3 *vect3, float bound);
extern void vect_bound_in_3d(struct FloatVect3 *vect3, float bound);
extern void vect_scale(struct FloatVect3 *vect3, float norm_des);
#ifdef __cplusplus
sw/airborne/modules/ctrl/approach_moving_target.c
+207
View File
@@ -0,0 +1,207 @@
/*
* Copyright (C) 2021 Ewoud Smeur <e.j.j.smeur@tudelft.nl>
*
* 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/approach_moving_target.c"
* @author Ewoud Smeur <e.j.j.smeur@tudelft.nl>
* Approach a moving target (e.g. ship)
*/
#include "approach_moving_target.h"
#include "generated/modules.h"
#include "modules/core/abi.h"
float amt_err_slowdown_gain = AMT_ERR_SLOWDOWN_GAIN;
float approach_moving_target_angle_deg;
#define DEBUG_AMT TRUE
#include <stdio.h>
struct Amt amt = {
.distance = 60,
.speed = -1.0,
.pos_gain = 0.3,
.psi_ref = 180.0,
.slope_ref = 35.0,
.speed_gain = 1.0,
.relvel_gain = 1.0,
.enabled_time = 0,
.wp_id = 0,
};
struct AmtTelem {
struct FloatVect3 des_pos;
struct FloatVect3 des_vel;
};
struct AmtTelem amt_telem;
bool approach_moving_target_enabled = false;
struct FloatVect3 nav_get_speed_sp_from_diagonal(struct EnuCoor_i target, float pos_gain, float rope_heading);
void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned);
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_approach_moving_target(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_APPROACH_MOVING_TARGET(trans, dev, AC_ID,
&amt_telem.des_pos.x,
&amt_telem.des_pos.y,
&amt_telem.des_pos.z,
&amt_telem.des_vel.x,
&amt_telem.des_vel.y,
&amt_telem.des_vel.z,
&amt.distance);
}
#endif
void approach_moving_target_init(void)
{
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_APPROACH_MOVING_TARGET, send_approach_moving_target);
#endif
}
// Update a waypoint such that you can see on the GCS where the drone wants to go
void update_waypoint(uint8_t wp_id, struct FloatVect3 * target_ned) {
// Update the waypoint
struct EnuCoor_f target_enu;
ENU_OF_TO_NED(target_enu, *target_ned);
waypoint_set_enu(wp_id, &target_enu);
// Send waypoint update every half second
RunOnceEvery(100/2, {
// Send to the GCS that the waypoint has been moved
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
&waypoints[wp_id].enu_i.x,
&waypoints[wp_id].enu_i.y,
&waypoints[wp_id].enu_i.z);
} );
}
// Function to enable from flight plan (call repeatedly!)
void approach_moving_target_enable(uint8_t wp_id) {
amt.enabled_time = get_sys_time_msec();
amt.wp_id = wp_id;
}
/**
* @brief Generates a velocity reference from a diagonal approach path
*
*/
void follow_diagonal_approach(void) {
// Check if the flight plan recently called the enable function
if ( (get_sys_time_msec() - amt.enabled_time) > (2000 / NAVIGATION_FREQUENCY)) {
return;
}
// Get the target position, velocity and heading
struct NedCoor_f target_pos, target_vel = {0};
float target_heading;
if(!target_get_pos(&target_pos, &target_heading)) {
// TODO: What to do? Same can be checked for the velocity
return;
}
target_get_vel(&target_vel);
VECT3_SMUL(target_vel, target_vel, amt.speed_gain);
// Reference model
float gamma_ref = RadOfDeg(amt.slope_ref);
float psi_ref = RadOfDeg(target_heading + amt.psi_ref);
amt.rel_unit_vec.x = cosf(gamma_ref) * cosf(psi_ref);
amt.rel_unit_vec.y = cosf(gamma_ref) * sinf(psi_ref);
amt.rel_unit_vec.z = -sinf(gamma_ref);
// desired position = rel_pos + target_pos
struct FloatVect3 ref_relpos;
VECT3_SMUL(ref_relpos, amt.rel_unit_vec, amt.distance);
// ATTENTION, target_pos is already relative now!
struct FloatVect3 des_pos;
VECT3_SUM(des_pos, ref_relpos, target_pos);
struct FloatVect3 ref_relvel;
VECT3_SMUL(ref_relvel, amt.rel_unit_vec, amt.speed * amt.relvel_gain);
// error controller
struct FloatVect3 pos_err;
struct FloatVect3 ec_vel;
struct NedCoor_f *drone_pos = stateGetPositionNed_f();
// ATTENTION, target_pos is already relative now!
// VECT3_DIFF(pos_err, des_pos, *drone_pos);
VECT3_COPY(pos_err, des_pos);
VECT3_SMUL(ec_vel, pos_err, amt.pos_gain);
// desired velocity = rel_vel + target_vel + error_controller(using NED position)
struct FloatVect3 des_vel = {
ref_relvel.x + target_vel.x + ec_vel.x,
ref_relvel.y + target_vel.y + ec_vel.y,
ref_relvel.z + target_vel.z + ec_vel.z,
};
vect_bound_in_3d(&des_vel, 10.0);
// Bound vertical speed setpoint
if(stateGetAirspeed_f() > 13.0) {
Bound(des_vel.z, -4.0, 5.0);
} else {
Bound(des_vel.z, -nav_climb_vspeed, -nav_descend_vspeed);
}
AbiSendMsgVEL_SP(VEL_SP_FCR_ID, &des_vel);
/* limit the speed such that the vertical component is small enough
* and doesn't outrun the vehicle
*/
float min_speed;
float sin_gamma_ref = sinf(gamma_ref);
if (sin_gamma_ref > 0.05) {
min_speed = (nav_descend_vspeed+0.1) / sin_gamma_ref;
} else {
min_speed = -5.0; // prevent dividing by zero
}
// The upper bound is not very important
Bound(amt.speed, min_speed, 4.0);
// Reduce approach speed if the error is large
float norm_pos_err_sq = VECT3_NORM2(pos_err);
float int_speed = amt.speed / (norm_pos_err_sq * amt_err_slowdown_gain + 1.0);
// integrate speed to get the distance
float dt = FOLLOW_DIAGONAL_APPROACH_PERIOD;
amt.distance += int_speed*dt;
// For display purposes
struct FloatVect3 disp_pos_target;
VECT3_SUM(disp_pos_target, des_pos, *drone_pos);
update_waypoint(amt.wp_id, &disp_pos_target);
// Debug target pos:
VECT3_DIFF(amt_telem.des_pos, *drone_pos, target_pos);
// Update values for telemetry
VECT3_COPY(amt_telem.des_pos, des_pos);
VECT3_COPY(amt_telem.des_vel, des_vel);
}
sw/airborne/modules/ctrl/approach_moving_target.h
+55
View File
@@ -0,0 +1,55 @@
/*
* Copyright (C) 2021 Ewoud Smeur <e.j.j.smeur@tudelft.nl>
*
* 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/approach_moving_target.h"
* @author Ewoud Smeur <e.j.j.smeur@tudelft.nl>
* Approach a moving target (e.g. ship)
*/
#ifndef APPROACH_MOVING_TARGET_H
#define APPROACH_MOVING_TARGET_H
#include "std.h"
#include "math/pprz_algebra_float.h"
// Angle of the approach in degrees
extern float approach_moving_target_angle_deg;
struct Amt {
struct FloatVect3 rel_unit_vec;
float distance;
float speed;
float pos_gain;
float psi_ref;
float slope_ref;
float speed_gain;
float relvel_gain;
int32_t enabled_time;
uint8_t wp_id;
};
extern struct Amt amt;
extern float amt_err_slowdown_gain;
extern void approach_moving_target_init(void);
extern void follow_diagonal_approach(void);
extern void approach_moving_target_enable(uint8_t wp_id);
#endif // APPROACH_MOVING_TARGET_H
sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
+184
View File
@@ -0,0 +1,184 @@
/*
* Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* 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/eff_scheduling_nederdrone.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Interpolation of control effectivenss matrix
of the Nederdrone.
If instead using online adaptation is an option, be sure to
not use this module at the same time!
*/
#include "modules/ctrl/eff_scheduling_nederdrone.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "state.h"
#include "autopilot.h"
#include "modules/radio_control/radio_control.h"
#include "generated/radio.h"
#include "modules/actuators/actuators.h"
#define INDI_SCHEDULING_LOWER_BOUND_G1 0.0001
// Airspeed at which tip props should be turned on
#ifndef INDI_SCHEDULING_LOW_AIRSPEED
#define INDI_SCHEDULING_LOW_AIRSPEED 12.0
#endif
bool all_act_fwd_sched = false;
int32_t use_scheduling = 1;
float thrust_eff_scaling = 1.0;
static float g_forward[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL_FWD, STABILIZATION_INDI_G1_PITCH_FWD, STABILIZATION_INDI_G1_YAW_FWD, STABILIZATION_INDI_G1_THRUST_FWD};
static float g_hover[4][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL, STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST};
// Functions to schedule switching on and of of tip props on front wing
float sched_ratio_tip_props = 1.0;
// If pitch lower, pitch props gradually switch off till sched_tip_prop_lower_pitch_limit_deg (1 > sched_ratio_tip_props > 0)
float sched_tip_prop_upper_pitch_limit_deg = -45;
// If pitch lower, pitch props switch fully off (sched_ratio_tip_props goes to 0)
float sched_tip_prop_lower_pitch_limit_deg = -65;
// Setting to not switch off tip props during forward flight
bool sched_tip_props_always_on = false;
void schdule_control_effectiveness(void);
void ctrl_eff_scheduling_init(void)
{
// your init code here
for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
g_hover[0][i] = g_hover[0][i] / INDI_G_SCALING;
g_hover[1][i] = g_hover[1][i] / INDI_G_SCALING;
g_hover[2][i] = g_hover[2][i] / INDI_G_SCALING;
g_hover[3][i] = g_hover[3][i] / INDI_G_SCALING;
g_forward[0][i] = g_forward[0][i] / INDI_G_SCALING;
g_forward[1][i] = g_forward[1][i] / INDI_G_SCALING;
g_forward[2][i] = g_forward[2][i] / INDI_G_SCALING;
g_forward[3][i] = g_forward[3][i] / INDI_G_SCALING;
}
}
void ctrl_eff_scheduling_periodic(void)
{
#ifdef SITL
sched_ratio_tip_props = 1.0;
#else
schdule_control_effectiveness();
#endif
}
/**
* Function that calculates control effectiveness values for the Nederdrone inner loop.
* Default requency: 20 Hz.
*/
void schdule_control_effectiveness(void) {
float airspeed = stateGetAirspeed_f();
float ratio = 0.0;
struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
// Ratio is only based on pitch now, as the pitot tube is often not mounted.
if (use_scheduling == 1) {
ratio = fabs(eulers_zxy.theta) / M_PI_2;
} else {
ratio = 0.0;
}
Bound(ratio,0.0,1.0);
float stall_speed = 14.0; // m/s
float pitch_ratio = 0.0;
// Assume hover or stalled conditions below 14 m/s
if (use_scheduling == 1) {
if ( (eulers_zxy.theta > -M_PI_4) || (airspeed < stall_speed) ) {
if (eulers_zxy.theta > -M_PI_4) {
pitch_ratio = 0.0;
} else {
pitch_ratio = fabs(1.0 - eulers_zxy.theta/(-M_PI_4));
}
} else {
pitch_ratio = 1.0;
}
} else {
pitch_ratio = 0.0;
}
Bound(pitch_ratio,0.0,1.0);
float airspeed_pitch_eff = airspeed;
Bound(airspeed_pitch_eff, 8.0, 30.0);
for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
// Roll
g1g2[0][i] = g_hover[0][i] * (1.0 - ratio) + g_forward[0][i] * ratio;
//Pitch, scaled with v^2
if (i>3 || all_act_fwd_sched) {
g1g2[1][i] = g_hover[1][i] * (1.0 - pitch_ratio) + g_forward[1][i] * pitch_ratio;
} else {
g1g2[1][i] = g_hover[1][i] * (1.0 - pitch_ratio) + g_forward[1][i] * pitch_ratio * airspeed_pitch_eff * airspeed_pitch_eff / (16.0*16.0);
}
//Yaw
g1g2[2][i] = g_hover[2][i] * (1.0 - ratio) + g_forward[2][i] * ratio;
// Determine thrust of the wing to adjust the effectiveness of servos
float wing_thrust_scaling = 1.0;
#if INDI_NUM_ACT != 8
#error "ctfl_eff_scheduling_nederdrone is very specific and only works for one Nederdrone configuration!"
#endif
if (i>3) {
float wing_thrust = actuators_pprz[i-4];
Bound(wing_thrust,3000.0,9600.0);
wing_thrust_scaling = wing_thrust/9600.0/0.8;
}
g1g2[0][i] *= wing_thrust_scaling;
g1g2[1][i] *= wing_thrust_scaling;
g1g2[2][i] *= wing_thrust_scaling;
}
// Thrust effectiveness
float ratio_spec_force = 0.0;
float airspeed_spec_force = airspeed;
Bound(airspeed_spec_force, 8.0, 20.0);
ratio_spec_force = (airspeed_spec_force-8.0) / 12.0;
for (uint8_t i = 0; i < INDI_NUM_ACT; i++) {
// Thrust
g1g2[3][i] = g_hover[3][i] * (1.0 - ratio_spec_force) + g_forward[3][i] * ratio_spec_force;
g1g2[3][i] *= thrust_eff_scaling;
}
bool low_airspeed = stateGetAirspeed_f() < INDI_SCHEDULING_LOW_AIRSPEED;
// Tip prop ratio
float pitch_deg = eulers_zxy.theta / M_PI * 180.f;
float pitch_range_deg = sched_tip_prop_upper_pitch_limit_deg - sched_tip_prop_lower_pitch_limit_deg;
if (sched_tip_props_always_on || low_airspeed || radio_control.values[RADIO_AUX2] > 0) {
sched_ratio_tip_props = 1.0;
} else {
float pitch_offset = pitch_deg - sched_tip_prop_lower_pitch_limit_deg;
sched_ratio_tip_props = pitch_offset / pitch_range_deg;
}
Bound(sched_ratio_tip_props, 0.0, 1.0);
}
sw/airborne/modules/ctrl/eff_scheduling_nederdrone.h
+53
View File
@@ -0,0 +1,53 @@
/*
* Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
*
* 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/eff_scheduling_nederdrone.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Interpolation of control effectivenss matrix
of the Nederdrone.
If instead using online adaptation is an option, be sure to
not use this module at the same time!
*/
#ifndef EFF_SCHEDULING_NEDERDRONE_H
#define EFF_SCHEDULING_NEDERDRONE_H
#include <stdio.h>
#include <stdbool.h>
extern void ctrl_eff_scheduling_init(void);
extern void ctrl_eff_scheduling_periodic(void);
// Functions to schedule switching on and of of tip props on front wing
extern float sched_ratio_tip_props;
// If pitch lower, pitch props gradually switch off till sched_tip_prop_lower_pitch_limit_deg (1 > sched_ratio_tip_props > 0)
extern float sched_tip_prop_upper_pitch_limit_deg;
// If pitch lower, pitch props switch fully off (sched_ratio_tip_props goes to 0)
extern float sched_tip_prop_lower_pitch_limit_deg;
// Setting to not switch off tip props during forward flight
extern bool sched_tip_props_always_on;
// Setting to scale the thrust effectiveness
extern float thrust_eff_scaling;
// Schedule all forward actuators with airspeed instead of only the flaps
extern bool all_act_fwd_sched;
#endif // EFF_SCHEDULING_NEDERDRONE_H
sw/airborne/modules/ctrl/target_pos.c
+229
View File
@@ -0,0 +1,229 @@
/*
* Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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/target_pos.c"
* @author Freek van Tienen <freek.v.tienen@gmail.com>
* Control a rotorcraft to follow at a defined distance from the target
*/
#include "target_pos.h"
#include <math.h>
#include "modules/datalink/telemetry.h"
#include "modules/core/abi.h"
// The timeout when receiving GPS messages from the ground in ms
#ifndef TARGET_POS_TIMEOUT
#define TARGET_POS_TIMEOUT 5000
#endif
// The timeout when recceiving an RTK gps message from the GPS
#ifndef TARGET_RTK_TIMEOUT
#define TARGET_RTK_TIMEOUT 1000
#endif
#ifndef TARGET_OFFSET_HEADING
#define TARGET_OFFSET_HEADING 180.0
#endif
#ifndef TARGET_OFFSET_DISTANCE
#define TARGET_OFFSET_DISTANCE 12.0
#endif
#ifndef TARGET_OFFSET_HEIGHT
#define TARGET_OFFSET_HEIGHT -1.2
#endif
#ifndef TARGET_INTEGRATE_XY
#define TARGET_INTEGRATE_XY true
#endif
#ifndef TARGET_INTEGRATE_Z
#define TARGET_INTEGRATE_Z false
#endif
/* Initialize the main structure */
struct target_t target = {
.pos = {0},
.offset = {
.heading = TARGET_OFFSET_HEADING,
.distance = TARGET_OFFSET_DISTANCE,
.height = TARGET_OFFSET_HEIGHT,
},
.target_pos_timeout = TARGET_POS_TIMEOUT,
.rtk_timeout = TARGET_RTK_TIMEOUT,
.integrate_xy = TARGET_INTEGRATE_XY,
.integrate_z = TARGET_INTEGRATE_Z
};
/* GPS abi callback */
static abi_event gps_ev;
static void gps_cb(uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s);
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_target_pos_info(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_TARGET_POS_INFO(trans, dev, AC_ID,
&target.pos.lla.lat,
&target.pos.lla.lon,
&target.pos.lla.alt,
&target.pos.ground_speed,
&target.pos.climb,
&target.pos.course,
&target.pos.heading,
&target.offset.heading,
&target.offset.distance,
&target.offset.height);
}
#endif
void target_pos_init(void)
{
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_TARGET_POS_INFO, send_target_pos_info);
#endif
AbiBindMsgGPS(ABI_BROADCAST, &gps_ev, gps_cb);
}
/* Get the GPS lla position */
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
target.gps_lla.lat = gps_s->lla_pos.lat;
target.gps_lla.lon = gps_s->lla_pos.lon;
target.gps_lla.alt = gps_s->lla_pos.alt;
}
/**
* Receive a TARGET_POS message from the ground
*/
void target_parse_target_pos(uint8_t *buf)
{
if(DL_TARGET_POS_ac_id(buf) != AC_ID)
return;
// Save the received values
target.pos.recv_time = get_sys_time_msec();
target.pos.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); // FIXME: need to get from the real GPS
target.pos.lla.lat = DL_TARGET_POS_lat(buf);
target.pos.lla.lon = DL_TARGET_POS_lon(buf);
target.pos.lla.alt = DL_TARGET_POS_alt(buf);
target.pos.ground_speed = DL_TARGET_POS_speed(buf);
target.pos.climb = DL_TARGET_POS_climb(buf);
target.pos.course = DL_TARGET_POS_course(buf);
target.pos.heading = DL_TARGET_POS_heading(buf);
target.pos.valid = true;
}
/**
* Get the current target position (NED) and heading
*/
bool target_get_pos(struct NedCoor_f *pos, float *heading) {
float time_diff = 0;
/* When we have a valid target_pos message, state ned is initialized and no timeout */
if(target.pos.valid && state.ned_initialized_i && (target.pos.recv_time+target.target_pos_timeout) > get_sys_time_msec()) {
struct NedCoor_i target_pos_cm, drone_pos_cm;
// Convert from LLA to NED using origin from the UAV
ned_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &target.pos.lla);
// Convert from LLA to NED using origin from the UAV
ned_of_lla_point_i(&drone_pos_cm, &state.ned_origin_i, &target.gps_lla);
// Convert to floating point (cm to meters)
pos->x = (target_pos_cm.x - drone_pos_cm.x) * 0.01;
pos->y = (target_pos_cm.y - drone_pos_cm.y) * 0.01;
pos->z = (target_pos_cm.z - drone_pos_cm.z) * 0.01;
// In seconds, overflow uint32_t in 49,7 days
time_diff = (get_sys_time_msec() - target.pos.recv_time) * 0.001; // FIXME: should be based on TOW of ground gps
// Return the heading
*heading = target.pos.heading;
// If we have a velocity measurement try to integrate the x-y position when enabled
struct NedCoor_f vel = {0};
bool got_vel = target_get_vel(&vel);
if(target.integrate_xy && got_vel) {
pos->x = pos->x + vel.x * time_diff;
pos->y = pos->y + vel.y * time_diff;
}
if(target.integrate_z && got_vel) {
pos->z = pos->z + vel.z * time_diff;
}
// Offset the target
pos->x += target.offset.distance * cosf((*heading + target.offset.heading)/180.*M_PI);
pos->y += target.offset.distance * sinf((*heading + target.offset.heading)/180.*M_PI);
pos->z -= target.offset.height;
return true;
}
return false;
}
/**
* Get the current target velocity (NED)
*/
bool target_get_vel(struct NedCoor_f *vel) {
/* When we have a valid target_pos message, state ned is initialized and no timeout */
if(target.pos.valid && state.ned_initialized_i && (target.pos.recv_time+target.target_pos_timeout) > get_sys_time_msec()) {
// Calculate baed on ground speed and course
vel->x = target.pos.ground_speed * cosf(target.pos.course/180.*M_PI);
vel->y = target.pos.ground_speed * sinf(target.pos.course/180.*M_PI);
vel->z = -target.pos.climb;
return true;
}
return false;
}
/**
* Set the current measured distance and heading as offset
*/
bool target_pos_set_current_offset(float unk __attribute__((unused))) {
if(target.pos.valid && state.ned_initialized_i && (target.pos.recv_time+target.target_pos_timeout) > get_sys_time_msec()) {
struct NedCoor_i target_pos_cm;
struct NedCoor_f uav_pos = *stateGetPositionNed_f();
// Convert from LLA to NED using origin from the UAV
ned_of_lla_point_i(&target_pos_cm, &state.ned_origin_i, &target.pos.lla);
// Convert to floating point (cm to meters)
struct NedCoor_f pos;
pos.x = target_pos_cm.x * 0.01;
pos.y = target_pos_cm.y * 0.01;
pos.z = target_pos_cm.z * 0.01;
target.offset.distance = sqrtf(powf(uav_pos.x - pos.x, 2) + powf(uav_pos.y - pos.y, 2));
target.offset.height = -(uav_pos.z - pos.z);
target.offset.heading = atan2f((uav_pos.y - pos.y), (uav_pos.x - pos.x))*180.0/M_PI - target.pos.heading;
}
return false;
}
sw/airborne/modules/ctrl/target_pos.h
+67
View File
@@ -0,0 +1,67 @@
/*
* Copyright (C) 2021 Freek van Tienen <freek.v.tienen@gmail.com>
*
* 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/target_pos.h"
* @author Freek van Tienen <freek.v.tienen@gmail.com>
* Create a target position derived from and RTK gps or TARGET_POS message
*/
#ifndef TARGET_POS_H
#define TARGET_POS_H
#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_geodetic_float.h"
struct target_pos_t {
bool valid; ///< If the data of the target position is valid
uint32_t recv_time; ///< Time of when the target position message was received [msec]
uint32_t tow; ///< Time of week of the target position measurement
struct LlaCoor_i lla; ///< Lat, lon and altitude position of the target
float ground_speed; ///< Ground speed of the target [m/s]
float course; ///< Ground course of the target [deg]
float heading; ///< Heading of the target [deg]
float climb; ///< Climb speed, z-up [m/s]
};
struct target_offset_t {
float heading; ///< Target offset heading
float distance; ///< Target offset distance
float height; ///< Target offset height
};
struct target_t {
struct target_pos_t pos; ///< The target position message
struct target_offset_t offset; ///< The target offset relative to ground heading
uint32_t target_pos_timeout; ///< Ground target position message timeout [msec]
uint32_t rtk_timeout; ///< RTK message timeout [msec]
bool integrate_xy; ///< Enable integration of the position in X-Y (North/East) frame
bool integrate_z; ///< Enable integration of the position in Z (Up) frame
struct LlaCoor_i gps_lla; ///< GPS LLA position
};
extern struct target_t target;
extern void target_pos_init(void);
extern void target_parse_target_pos(uint8_t *buf);
extern bool target_get_pos(struct NedCoor_f *pos, float *heading);
extern bool target_get_vel(struct NedCoor_f *vel);
extern bool target_pos_set_current_offset(float unk);
#endif
sw/ext/pprzlink
+1 -1
Submodule sw/ext/pprzlink updated: e4e61bb7aa...50441e04cb