mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-02 13:27:32 +08:00
[combined] Small fixes (#2750)
* [conf] Add NederDrone Trailing edge motors * Update the enter function of guidance_indi_hybrid * added guidance hybrid message * [ekf2] Fix EKF2 capabilities to fly with optitrack system * [ground] Add Herelink python script for logging RSSI Co-authored-by: Ewoud Smeur <e.j.j.smeur@tudelft.nl>
This commit is contained in:
Freek van Tienen
GitHub
@@ -0,0 +1,360 @@
|
||||
<!DOCTYPE airframe SYSTEM "../airframe.dtd">
|
||||
|
||||
<!-- This is a Nedderdrone with Trailing edge motors
|
||||
* Airframe: TUD00289
|
||||
* Autopilot: Pixhawk 4
|
||||
* Actuators: 12x T-Motor ESC + Motors and 8x Servos (all CAN)
|
||||
* Datalink: Herelink
|
||||
* GPS: UBlox F9P
|
||||
* RC: SBUS Crossfire
|
||||
-->
|
||||
|
||||
<airframe name="Neddrone4">
|
||||
<description>Neddrone4</description>
|
||||
|
||||
<firmware name="rotorcraft">
|
||||
<target name="ap" board="px4fmu_5.0_chibios">
|
||||
<configure name="PERIODIC_FREQUENCY" value="500"/>
|
||||
<configure name="FLASH_MODE" value="SWD"/>
|
||||
|
||||
<module name="radio_control" type="sbus">
|
||||
<configure name="SBUS_PORT" value="UART3"/>
|
||||
</module>
|
||||
|
||||
<module name="airspeed_ets.xml">
|
||||
<configure name="AIRSPEED_ETS_I2C_DEV" value="I2C4"/>
|
||||
<define name="AIRSPEED_ETS_SCALE" value="1.24"/>
|
||||
</module>
|
||||
|
||||
<module name="scheduling_indi_simple"/>
|
||||
|
||||
<!-- 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"/>
|
||||
|
||||
<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"/>
|
||||
</target>
|
||||
|
||||
<target name="nps" board="pc">
|
||||
<module name="radio_control" type="datalink"/>
|
||||
<module name="fdm" type="jsbsim"/>
|
||||
|
||||
<module name="scheduling_indi_simple"/>
|
||||
|
||||
<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="telemetry" type="transparent">
|
||||
<configure name="MODEM_BAUD" value="B115200"/>
|
||||
</module>
|
||||
|
||||
<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"/>
|
||||
<module name="gps" type="ubx_ucenter"/-->
|
||||
<module name="stabilization" type="indi_simple"/>
|
||||
<module name="stabilization" type="rate_indi"/>
|
||||
<module name="ins" type="ekf2" />
|
||||
|
||||
<module name="air_data"/>
|
||||
<module name="send_imu_mag_current"/>
|
||||
|
||||
<!-- 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.2"/>
|
||||
<define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
|
||||
<define name="GUIDANCE_INDI_POS_GAINZ" value="0.2"/>
|
||||
<define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.0"/>
|
||||
<define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
|
||||
<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="16.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="2500"/>
|
||||
</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="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_3" no="2" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_4" no="3" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_5" no="4" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_6" no="5" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="AIL_1" no="6" min="-3000" neutral="0" max="3000"/>
|
||||
<servo name="FLAP_1" no="7" min="-3000" neutral="0" max="3000"/>
|
||||
<servo name="FLAP_2" no="8" min="3000" neutral="0" max="-3000"/>
|
||||
<servo name="AIL_2" no="9" min="3000" neutral="0" max="-3000"/>
|
||||
</servos>
|
||||
|
||||
<!-- CAN BUS 2 (Back Wing) -->
|
||||
<servos driver="Uavcan2">
|
||||
<servo name="MOTOR_7" no="0" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_8" no="1" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_9" no="2" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_10" no="3" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_11" no="4" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="MOTOR_12" no="5" min="-8191" neutral="1500" max="8191"/>
|
||||
<servo name="AIL_3" no="6" min="6000" neutral="0" max="-6000"/>
|
||||
<servo name="FLAP_3" no="7" min="6000" neutral="0" max="-6000"/>
|
||||
<servo name="FLAP_4" no="8" min="-6000" neutral="0" max="6000"/>
|
||||
<servo name="AIL_4" no="9" 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, 253, 159, -159, -253, -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>
|
||||
<call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
|
||||
<let var="th_hold" value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
|
||||
|
||||
<set servo="MOTOR_1" value="($th_hold? -9600 : motor_mixing.commands[0])"/>
|
||||
<set servo="MOTOR_2" value="($th_hold? -9600 : motor_mixing.commands[1])"/>
|
||||
<set servo="MOTOR_3" value="($th_hold? -9600 : motor_mixing.commands[2])"/>
|
||||
<set servo="MOTOR_4" value="($th_hold? -9600 : motor_mixing.commands[3])"/>
|
||||
<set servo="MOTOR_5" value="($th_hold? -9600 : motor_mixing.commands[4])"/>
|
||||
<set servo="MOTOR_6" value="($th_hold? -9600 : motor_mixing.commands[5])"/>
|
||||
<set servo="MOTOR_7" value="($th_hold? -9600 : motor_mixing.commands[6])"/>
|
||||
<set servo="MOTOR_8" value="($th_hold? -9600 : motor_mixing.commands[7])"/>
|
||||
<set servo="MOTOR_9" value="($th_hold? -9600 : motor_mixing.commands[8])"/>
|
||||
<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*@YAW"/>
|
||||
<set servo="AIL_2" value="-2*@YAW"/>
|
||||
<set servo="AIL_3" value=" 2*@YAW"/>
|
||||
<set servo="AIL_4" value="-2*@YAW"/>
|
||||
<set servo="FLAP_1" value=" 2*@YAW"/>
|
||||
<set servo="FLAP_2" value="-2*@YAW"/>
|
||||
<set servo="FLAP_3" value=" 2*@YAW"/>
|
||||
<set servo="FLAP_4" value="-2*@YAW"/>
|
||||
</command_laws>
|
||||
|
||||
<section name="MISC">
|
||||
<define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 17.9024749557 * adc)"/><!-- TODO: verify/calibrate -->
|
||||
<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.5"/>
|
||||
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
|
||||
|
||||
<!-- 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"/>
|
||||
|
||||
<define name="INS_EKF2_FUSION_MODE" value="MASK_USE_GPS | MASK_USE_GPSYAW"/>
|
||||
<define name="INS_EKF2_VDIST_SENSOR_TYPE" value="VDIST_SENSOR_GPS"/>
|
||||
<define name="INS_EKF2_GPS_CHECK_MASK" value="0"/>
|
||||
<define name="INS_EKF2_GPS_COURSE_YAW" 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 (for Pixhawk 4) -->
|
||||
<define name="MPU_CHAN_X" value="1"/>
|
||||
<define name="MPU_CHAN_Y" value="0"/>
|
||||
<define name="MPU_CHAN_Z" value="2"/>
|
||||
<define name="MPU_X_SIGN" value="1"/>
|
||||
<define name="MPU_Y_SIGN" value="1"/>
|
||||
<define name="MPU_Z_SIGN" value="-1"/>
|
||||
|
||||
<!-- Calibrated in the MAVLab 14-05-2020 -->
|
||||
<define name="ACCEL_X_NEUTRAL" value="-337"/>
|
||||
<define name="ACCEL_Y_NEUTRAL" value="64"/>
|
||||
<define name="ACCEL_Z_NEUTRAL" value="-25"/>
|
||||
<define name="ACCEL_X_SENS" value="4.670307671109528" integer="16"/>
|
||||
<define name="ACCEL_Y_SENS" value="4.9016250738902425" integer="16"/>
|
||||
<define name="ACCEL_Z_SENS" value="4.846689188075245" integer="16"/>
|
||||
|
||||
<!-- Calibrated at valkenburg 20-05-2020 (external magnetometer) -->
|
||||
<define name="MAG_X_NEUTRAL" value="866"/>
|
||||
<define name="MAG_Y_NEUTRAL" value="-1530"/>
|
||||
<define name="MAG_Z_NEUTRAL" value="-3313"/>
|
||||
<define name="MAG_X_SENS" value="0.6067461130451115" integer="16"/>
|
||||
<define name="MAG_Y_SENS" value="0.6544292255627779" integer="16"/>
|
||||
<define name="MAG_Z_SENS" value="0.6352539557433349" integer="16"/>
|
||||
|
||||
<!-- Define axis in hover frame -->
|
||||
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
|
||||
<define name="BODY_TO_IMU_THETA" value="90." 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="90." unit="deg/s"/>
|
||||
<define name="DEADBAND_R" value="200"/>
|
||||
<define name="SP_PSI_DELTA_LIMIT" value="45" unit="deg"/>
|
||||
</section>
|
||||
|
||||
<section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
|
||||
<!-- control effectiveness (hover) -->
|
||||
<define name="G1_P" value="0.0030"/>
|
||||
<define name="G1_Q" value="0.0035"/>
|
||||
<define name="G1_R" value="0.0004"/>
|
||||
<define name="G2_R" value="0.00015"/>
|
||||
|
||||
<!-- control effectiveness (forward) -->
|
||||
<define name="FORWARD_G1_P" value="0.0020"/>
|
||||
<define name="FORWARD_G1_Q" value="0.0077"/>
|
||||
<define name="FORWARD_G1_R" value="0.004"/>
|
||||
|
||||
<!-- 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="50.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.0"/>
|
||||
|
||||
<!-- first order actuator dynamics -->
|
||||
<define name="ACT_DYN_P" value="0.0354"/>
|
||||
<define name="ACT_DYN_Q" value="0.0354"/>
|
||||
<define name="ACT_DYN_R" value="0.0354"/>
|
||||
|
||||
<!-- 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="30" 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="m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,ail1,ail2,ail3,ail4,flap1,flap2,flap3,flap4" type="string[]"/>
|
||||
<define name="JSBSIM_MODEL" value="nederdrone" type="string"/>
|
||||
<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"/>
|
||||
</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>
|
||||
@@ -0,0 +1,75 @@
|
||||
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
|
||||
|
||||
<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Nederdrone Cyberzoo" security_height="0.4">
|
||||
<header>
|
||||
#include "subsystems/datalink/datalink.h"
|
||||
#include "subsystems/electrical.h"
|
||||
#include "subsystems/radio_control.h"
|
||||
#include "subsystems/ahrs.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
|
||||
#include "firmwares/rotorcraft/navigation.h"
|
||||
</header>
|
||||
<waypoints>
|
||||
<waypoint lat="51.990631" lon="4.376808" name="HOME"/>
|
||||
<waypoint name="CLIMB" x="-0.7" y="-0.6"/>
|
||||
<waypoint name="STDBY" x="0.5" y="0.3"/>
|
||||
<waypoint name="ROPE" x="-2.4" y="1.3"/>
|
||||
<waypoint name="TD" x="0.6" y="-1.3"/>
|
||||
<waypoint lat="51.9905834" lon="4.3767710" name="_CZ1"/>
|
||||
<waypoint lat="51.9906440" lon="4.3767060" name="_CZ2"/>
|
||||
<waypoint lat="51.9906860" lon="4.3768080" name="_CZ3"/>
|
||||
<waypoint lat="51.9906238" lon="4.3768729" name="_CZ4"/>
|
||||
</waypoints>
|
||||
<sectors>
|
||||
<sector color="blue" name="CyberZoo">
|
||||
<corner name="_CZ1"/>
|
||||
<corner name="_CZ2"/>
|
||||
<corner name="_CZ3"/>
|
||||
<corner name="_CZ4"/>
|
||||
</sector>
|
||||
</sectors>
|
||||
<variables>
|
||||
<!-- <variable var="rope_heading" init="0.0f" type="float" min="-180.0" max="180.0" step="0.1"/> -->
|
||||
<variable var="rope_heading" init="0.0f" type="float" min="0." max="10." step="0.1"/>
|
||||
</variables>
|
||||
<blocks>
|
||||
<block name="Wait GPS">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<while cond="!GpsFixValid()"/>
|
||||
</block>
|
||||
<block name="Geo init">
|
||||
<while cond="LessThan(NavBlockTime(), 2)"/>
|
||||
<call_once fun="NavSetAltitudeReferenceHere()"/>
|
||||
</block>
|
||||
<block name="Holding point">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<call_once fun="NavSetWaypointHere(WP_ROPE)"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
<block key="r" name="Start Engine">
|
||||
<call_once fun="NavResurrect()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
<call_once fun="NavResurrect()"/>
|
||||
</block>
|
||||
<block name="TakeoffLow" strip_button="Takeoff" strip_icon="takeoff.png">
|
||||
<call_once fun="nav_set_heading_current()"/>
|
||||
<!-- <set value="DegOfRad(stateGetNedToBodyEulers_f()->psi)" var="rope_heading"/> -->
|
||||
<set var="rope_heading" value="stateGetNedToBodyEulers_f()->psi"/>
|
||||
<attitude pitch="-20." roll="0" throttle="0.85" until="stage_time>1" vmode="throttle"/>
|
||||
</block>
|
||||
<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png">
|
||||
<call_once fun="NavSetWaypointHere(WP_STDBY)"/>
|
||||
<stay wp="STDBY"/>
|
||||
</block>
|
||||
<block name="Approach Rope">
|
||||
<!--set value="1" var="approaching_rope"/-->
|
||||
<set value="1" var="take_heading_control"/>
|
||||
<call_once fun="nav_set_heading_rad(rope_heading)"/>
|
||||
<stay alt="0.15+WaypointAlt(WP_ROPE)" wp="ROPE"/>
|
||||
</block>
|
||||
<block name="Landed">
|
||||
<call_once fun="NavKillThrottle()"/>
|
||||
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
|
||||
</block>
|
||||
</blocks>
|
||||
</flight_plan>
|
||||
@@ -5,6 +5,17 @@
|
||||
<description>
|
||||
simple INS and AHRS using EKF2 from PX4
|
||||
</description>
|
||||
<define name="INS_EKF2_FUSION_MODE" value="(MASK_USE_GPS)" description="The sensor that are used for position fusion"/>
|
||||
<define name="INS_EKF2_VDIST_SENSOR_TYPE" value="VDIST_SENSOR_BARO" description="Primary sensor used for vertical distance"/>
|
||||
<define name="INS_EKF2_GPS_CHECK_MASK" value="(MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)" description="GPS checks enabled before initialization of the global positioning"/>
|
||||
<define name="INS_SONAR_MIN_RANGE" value="0.001" description="Minimum range in meters for the AGL sensor"/>
|
||||
<define name="INS_SONAR_MAX_RANGE" value="4.0" description="Maximum range in meters for the AGL sensor"/>
|
||||
<define name="INS_EKF2_AGL_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for AGL measurements"/>
|
||||
<define name="INS_EKF2_BARO_ID" value="ABI_BROADCAST" description="ABI sensor ID used ad input for Barometric measurements"/>
|
||||
<define name="INS_EKF2_GYRO_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for gyro measurements"/>
|
||||
<define name="INS_EKF2_ACCEL_ID" value="ABI_BROADCAST" description="ABI sensor ID used ad input for acceleration measurements"/>
|
||||
<define name="INS_EKF2_MAG_ID" value="ABI_BROADCAST" description="ABI sensor ID used as input for magnetic measurements"/>
|
||||
<define name="INS_EKF2_GPS_ID" value="ABI_BROADCAST" description="ABI sensor ID used ad input for GPS measurements"/>
|
||||
</doc>
|
||||
<settings>
|
||||
<dl_settings NAME="INS">
|
||||
|
||||
@@ -218,6 +218,7 @@
|
||||
<message name="AHRS_BIAS" period="0.5"/>
|
||||
<message name="AHRS_REF_QUAT" period="0.01"/>
|
||||
<message name="GUIDANCE_H_REF_INT" period="0.02"/>
|
||||
<message name="GUIDANCE_INDI_HYBRID" period="0.02"/>
|
||||
<message name="ESC" period="0.02"/>
|
||||
<message name="STAB_ATTITUDE_INDI" period="0.002"/>
|
||||
<message name="PPM" period="0.05"/>
|
||||
|
||||
@@ -288,12 +288,12 @@
|
||||
<aircraft
|
||||
name="Neddrone4"
|
||||
ac_id="14"
|
||||
airframe="airframes/tudelft/neddrone4.xml"
|
||||
airframe="airframes/tudelft/neddrone4_tem.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_cyberzoo.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/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/gps.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
|
||||
gui_color="blue"
|
||||
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
|
||||
/>
|
||||
@@ -303,9 +303,9 @@
|
||||
airframe="airframes/tudelft/neddrone5.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_waal.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/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/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/nav_basic_rotorcraft.xml modules/scheduling_indi_simple.xml modules/stabilization_indi_simple.xml"
|
||||
gui_color="blue"
|
||||
release="c52a0b7e581c74b42ecc9f9d712324e3ab1fcc5e"
|
||||
/>
|
||||
|
||||
@@ -200,8 +200,18 @@ void guidance_indi_init(void)
|
||||
* Call upon entering indi guidance
|
||||
*/
|
||||
void guidance_indi_enter(void) {
|
||||
thrust_in = 0.0;
|
||||
thrust_act = 0;
|
||||
thrust_in = stabilization_cmd[COMMAND_THRUST];
|
||||
thrust_act = thrust_in;
|
||||
|
||||
float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
|
||||
float sample_time = 1.0 / PERIODIC_FREQUENCY;
|
||||
for (int8_t i = 0; i < 3; i++) {
|
||||
init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
|
||||
}
|
||||
init_butterworth_2_low_pass(&roll_filt, tau, sample_time, stateGetNedToBodyEulers_f()->phi);
|
||||
init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, stateGetNedToBodyEulers_f()->theta);
|
||||
init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
|
||||
init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
|
||||
}
|
||||
|
||||
#include "firmwares/rotorcraft/navigation.h"
|
||||
|
||||
@@ -41,10 +41,10 @@ struct GpsState gps_datalink;
|
||||
void gps_datalink_init(void)
|
||||
{
|
||||
gps_datalink.fix = GPS_FIX_NONE;
|
||||
gps_datalink.pdop = 0;
|
||||
gps_datalink.sacc = 0;
|
||||
gps_datalink.pacc = 0;
|
||||
gps_datalink.cacc = 0;
|
||||
gps_datalink.pdop = 10;
|
||||
gps_datalink.sacc = 5;
|
||||
gps_datalink.pacc = 1;
|
||||
gps_datalink.cacc = 1;
|
||||
|
||||
gps_datalink.comp_id = GPS_DATALINK_ID;
|
||||
|
||||
@@ -108,7 +108,7 @@ static void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t
|
||||
gps_datalink.course = ((int32_t)heading) * 1e3;
|
||||
SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
|
||||
|
||||
gps_datalink.num_sv = 7;
|
||||
gps_datalink.num_sv = 30;
|
||||
gps_datalink.tow = tow;
|
||||
gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true
|
||||
|
||||
|
||||
@@ -47,6 +47,24 @@
|
||||
#error INS initialization from flight plan is not yet supported
|
||||
#endif
|
||||
|
||||
/** The EKF2 fusion mode setting */
|
||||
#ifndef INS_EKF2_FUSION_MODE
|
||||
#define INS_EKF2_FUSION_MODE (MASK_USE_GPS)
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(INS_EKF2_FUSION_MODE)
|
||||
|
||||
/** The EKF2 primary vertical distance sensor type */
|
||||
#ifndef INS_EKF2_VDIST_SENSOR_TYPE
|
||||
#define INS_EKF2_VDIST_SENSOR_TYPE VDIST_SENSOR_BARO
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(INS_EKF2_VDIST_SENSOR_TYPE)
|
||||
|
||||
/** The EKF2 GPS checks before initialization */
|
||||
#ifndef INS_EKF2_GPS_CHECK_MASK
|
||||
#define INS_EKF2_GPS_CHECK_MASK (MASK_GPS_NSATS | MASK_GPS_HACC | MASK_GPS_SACC)
|
||||
#endif
|
||||
PRINT_CONFIG_VAR(INS_EKF2_GPS_CHECK_MASK)
|
||||
|
||||
/** default AGL sensor to use in INS */
|
||||
#ifndef INS_EKF2_AGL_ID
|
||||
#define INS_EKF2_AGL_ID ABI_BROADCAST
|
||||
@@ -143,7 +161,7 @@ static void ins_ekf2_publish_attitude(uint32_t stamp);
|
||||
|
||||
/* Static local variables */
|
||||
static Ekf ekf; ///< EKF class itself
|
||||
static parameters *ekf_params; ///< The EKF parameters
|
||||
static parameters *ekf_params; ///< The EKF parameters
|
||||
struct ekf2_t ekf2; ///< Local EKF2 status structure
|
||||
static uint8_t ahrs_ekf2_id = AHRS_COMP_ID_EKF2; ///< Component ID for EKF
|
||||
|
||||
@@ -252,6 +270,9 @@ void ins_ekf2_init(void)
|
||||
ekf_params = ekf.getParamHandle();
|
||||
ekf_params->mag_fusion_type = MAG_FUSE_TYPE_HEADING;
|
||||
ekf_params->is_moving_scaler = 0.8f;
|
||||
ekf_params->fusion_mode = INS_EKF2_FUSION_MODE;
|
||||
ekf_params->vdist_sensor_type = INS_EKF2_VDIST_SENSOR_TYPE;
|
||||
ekf_params->gps_check_mask = INS_EKF2_GPS_CHECK_MASK;
|
||||
|
||||
/* Initialize struct */
|
||||
ekf2.ltp_stamp = 0;
|
||||
@@ -553,8 +574,13 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
|
||||
gps_msg.lat = gps_s->lla_pos.lat;
|
||||
gps_msg.lon = gps_s->lla_pos.lon;
|
||||
gps_msg.alt = gps_s->hmsl;
|
||||
#if INS_EKF2_GPS_COURSE_YAW
|
||||
gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
|
||||
gps_msg.yaw_offset = 0;
|
||||
#else
|
||||
gps_msg.yaw = NAN;
|
||||
gps_msg.yaw_offset = NAN;
|
||||
#endif
|
||||
gps_msg.fix_type = gps_s->fix;
|
||||
gps_msg.eph = gps_s->hacc / 100.0;
|
||||
gps_msg.epv = gps_s->vacc / 100.0;
|
||||
|
||||
Executable
+80
@@ -0,0 +1,80 @@
|
||||
#!/usr/bin/env python3
|
||||
#
|
||||
# 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/>.
|
||||
#
|
||||
|
||||
'''
|
||||
Read MAVLink messages from the herelink to get RSSI information
|
||||
'''
|
||||
|
||||
|
||||
from __future__ import print_function
|
||||
|
||||
import sys
|
||||
from os import path, getenv
|
||||
from time import time, sleep
|
||||
import numpy as np
|
||||
from collections import deque
|
||||
import argparse
|
||||
from pymavlink import mavutil
|
||||
|
||||
# if PAPARAZZI_HOME not set, then assume the tree containing this
|
||||
# file is a reasonable substitute
|
||||
PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
|
||||
sys.path.append(PPRZ_HOME + "/var/lib/python")
|
||||
from pprzlink.ivy import IvyMessagesInterface
|
||||
from pprzlink.message import PprzMessage
|
||||
|
||||
# parse args
|
||||
parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
parser.add_argument('-ac', '--ac_id', dest='ac_id', type=int, help='Aircraft ID to save the RSSI information to')
|
||||
parser.add_argument('-b', '--ivy_bus', dest='ivy_bus', help="Ivy bus address and port")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.ac_id is None:
|
||||
print("You need to define an aircraft ID")
|
||||
exit()
|
||||
|
||||
# start ivy interface
|
||||
if args.ivy_bus is not None:
|
||||
ivy = IvyMessagesInterface("herelink2ivy", ivy_bus=args.ivy_bus)
|
||||
else:
|
||||
ivy = IvyMessagesInterface("herelink2ivy")
|
||||
|
||||
# Start a connection listening to a UDP port
|
||||
conn = mavutil.mavlink_connection('udpin:0.0.0.0:14550')
|
||||
|
||||
print("Starting Herelink RSSI for ac_id %d" % (args.ac_id))
|
||||
try:
|
||||
# Start up the streaming client.
|
||||
while True:
|
||||
msg = conn.recv_match(type='RADIO_STATUS', blocking=True)
|
||||
if not msg or msg.get_type() == "BAD_DATA":
|
||||
continue
|
||||
|
||||
pmsg = PprzMessage("telemetry", "RSSI_COMBINED")
|
||||
pmsg['remote_rssi'] = msg.remrssi
|
||||
pmsg['tx_power'] = 0
|
||||
pmsg['local_rssi'] = msg.rssi
|
||||
pmsg['local_noise'] = msg.noise
|
||||
pmsg['remote_noise'] = msg.remnoise
|
||||
ivy.send(pmsg, args.ac_id)
|
||||
except (KeyboardInterrupt, SystemExit):
|
||||
print("Shutting down ivy interface...")
|
||||
ivy.shutdown()
|
||||
Reference in New Issue
Block a user