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 26 Packages Projects Releases Wiki Activity

ONELOOP controller updates (#3333)

Browse Source
This commit is contained in:
Tomaso Maria Luigi De Ponti
2024-07-16 13:28:31 +02:00
committed by GitHub
parent e089fd48f7
commit ecb94ad478
18 changed files with 4204 additions and 729 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml
+5 -4
View File
@@ -20,8 +20,8 @@
<module name="actuators" type="bebop"/> <module name="actuators" type="bebop"/>
<module name="imu" type="bebop"/> <module name="imu" type="bebop"/>
<!-- <module name="gps" type="furuno"/> --> <module name="gps" type="furuno"/>
<module name="gps" type="datalink"/> <!-- <module name="gps" type="datalink"/> -->
<module name="oneloop" type="andi"> <module name="oneloop" type="andi">
<configure name="ANDI_NUM_ACT" value="4"/> <configure name="ANDI_NUM_ACT" value="4"/>
@@ -33,9 +33,10 @@
<configure name="INDI_NUM_ACT" value="4"/> <configure name="INDI_NUM_ACT" value="4"/>
</module> </module>
<module name="ins" type="ekf2"> <!-- <module name="ins" type="ekf2">
<define name="INS_EKF2_OPTITRACK" value="true"/> <define name="INS_EKF2_OPTITRACK" value="true"/>
</module> </module> -->
<module name="ins" type="ekf2"/>
<module name="nav_hybrid"/> <module name="nav_hybrid"/>
<module name="guidance" type="indi"> <module name="guidance" type="indi">
conf/airframes/tudelft/rot_wing_v3c_oneloop.xml
+104 -81
View File
@@ -7,10 +7,10 @@
--> -->
<airframe name="RotatingWingV3C"> <airframe name="RotatingWingV3C">
<description>RotatingWingV3C for outdoor flight and simulation with INS EKF2</description> <description>RotatingWingV3C for outdoor flight with INS EKF2</description>
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<autopilot name="rotorcraft_oneloop_andi_indi.xml"/> <autopilot name="rotorcraft_oneloop_switch.xml"/>
<target name="ap" board="cube_orange"> <target name="ap" board="cube_orange">
<configure name="PERIODIC_FREQUENCY" value="500"/> <!-- Configure the main periodic frequency to 500Hz --> <configure name="PERIODIC_FREQUENCY" value="500"/> <!-- Configure the main periodic frequency to 500Hz -->
<configure name="NAVIGATION_FREQUENCY" value="500"/> <configure name="NAVIGATION_FREQUENCY" value="500"/>
@@ -28,18 +28,23 @@
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/> <define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/> <define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="RADIO_FMODE" value="RADIO_AUX2"/> <define name="RADIO_FMODE" value="RADIO_AUX2"/>
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <define name="AP_MODE_SWITCH" value="RADIO_AUX3"/>
<define name="RADIO_CONTROL_THRUST_X" value="RADIO_AUX4"/> <define name="RADIO_CONTROL_THRUST_X" value="RADIO_AUX4"/>
<!-- EKF EXT POSE configure inputs --> <!-- EKF EKF2 configure inputs -->
<define name="INS_EXT_POSE_MAG_ID" vaue="MAG_RM3100_SENDER_ID"/> <define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="INS_EXT_POSE_IMU_ID" value="IMU_CUBE1_ID"/> <define name="INS_EKF2_IMU_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<!--Only send gyro and accel that is being used--> <!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/> <define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/> <define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<!--Use adc rot sensor--> <!--Use adc rot sensor-->
<module name="wing_rotation_adc_sensor"> <module name="wing_rotation_adc_sensor">
<define name="ADC_OFFSET" value="-49.1731"/> <define name="ADC_WING_ROT_OFFSET" value="-49.1731"/>
<define name="ADC_SCALE" value="0.0029986"/> <define name="ADC_WING_ROT_SCALE" value="0.0029986"/>
</module> </module>
<!-- Log in high speed (Remove for outdoor flights) --> <!-- Log in high speed (Remove for outdoor flights) -->
<!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> --> <!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> -->
@@ -55,8 +60,8 @@
<!--Not dealing with these in the simulation--> <!--Not dealing with these in the simulation-->
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws --> <define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select --> <define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control --> <define name="AP_MODE_SWITCH" value="RADIO_AUX7"/> <!-- Switch between AP controllers -->
<define name="RADIO_KILL_SWITCH" value="5"/> <!-- Kill switch --> <define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/> <!-- Kill switch -->
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
</target> </target>
@@ -107,36 +112,35 @@
<!-- Control --> <!-- Control -->
<module name="stabilization" type="oneloop" > <module name="stabilization" type="oneloop" >
<configure name="INDI_NUM_ACT" value="8"/> <configure name="INDI_NUM_ACT" value="9"/>
</module> </module>
<module name="guidance" type="oneloop"/> <module name="guidance" type="oneloop"/>
<module name="nav" type="hybrid"> <module name="nav" type="hybrid">
<define name="NAV_HYBRID_MAX_AIRSPEED" value="5.0f"/> <define name="NAV_HYBRID_MAX_AIRSPEED" value="19.0f"/>
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/> <define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
<define name="NAV_HYBRID_MAX_ACCELERATION" value="4.0f"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/> <define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
<define name="GUIDANCE_H_USE_REF" value="FALSE"/> <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
</module> </module>
<module name="oneloop" type="andi"> <module name="oneloop" type="andi">
<configure name="ANDI_NUM_ACT_TOT" value="7"/>
<configure name="ANDI_NUM_ACT" value="5"/>
<configure name="ANDI_NUM_VIRTUAL_ACT" value="2"/>
<configure name="ANDI_OUTPUTS" value="6"/> <configure name="ANDI_OUTPUTS" value="6"/>
</module> </module>
<module name="eff_scheduling_rot_wing_V2"/>
<module name="wls"> <module name="wls">
<define name="WLS_N_U" value = "7"/> <define name="WLS_N_U" value = "11"/>
<define name="WLS_N_V" value = "6"/> <define name="WLS_N_V" value = "6"/>
</module> </module>
<module name="ground_detect_sensor"/> <module name="ground_detect_sensor"/>
<!-- <module name="rotwing_state"> <module name="rotwing_state_V2"/>
<define name = "ROT_MECH_IDX" value = "9"/>
<define name = "USE_ROTMECH_VIRTUAL" value = "FALSE"/>
<define name = "ROTMECH_DYN" value = "3.0"/>
</module> -->
<module name="agl_dist"/> <module name="agl_dist"/>
<module name="rot_wing_automation">
<define name="USE_V2" value="TRUE"/>
</module>
</firmware> </firmware>
<!-- PWM actuators --> <!-- PWM actuators -->
@@ -155,8 +159,8 @@
<!-- CAN BUS 1 command outputs--> <!-- CAN BUS 1 command outputs-->
<servos driver="Uavcan1Cmd"> <servos driver="Uavcan1Cmd">
<servo no="5" name="SERVO_ELEVATOR" min="3250" neutral="3250" max="-3250"/> <servo no="5" name="SERVO_ELEVATOR" min="5737" neutral="5737" max="-3936"/>
<servo no="6" name="SERVO_RUDDER" min="-3250" neutral="0" max="3250"/> <servo no="6" name="SERVO_RUDDER" min="-5225" neutral="0" max="5225"/>
</servos> </servos>
<!-- Can bus 2 actuators --> <!-- Can bus 2 actuators -->
@@ -176,42 +180,47 @@
</servos> </servos>
<commands> <commands>
<axis NAME="ROLL" FAILSAFE_VALUE="0"/> <!-- Real actuators commands -->
<axis NAME="PITCH" FAILSAFE_VALUE="0"/> <axis name="MOTOR_FRONT" group ="REAL" failsafe_value="0"/> <!-- IDX 0-->
<axis NAME="YAW" FAILSAFE_VALUE="0"/> <axis name="MOTOR_RIGHT" group ="REAL" failsafe_value="0"/> <!-- IDX 1-->
<axis NAME="THRUST" FAILSAFE_VALUE="0"/> <axis name="MOTOR_BACK" group ="REAL" failsafe_value="0"/> <!-- IDX 2-->
<axis name="MOTOR_LEFT" group ="REAL" failsafe_value="0"/> <!-- IDX 3-->
<axis name="MOTOR_PUSHER" group ="REAL" failsafe_value="0"/> <!-- IDX 4-->
<axis name="ELEVATOR" group ="REAL" failsafe_value="0"/> <!-- IDX 5-->
<axis name="RUDDER" group ="REAL" failsafe_value="0"/> <!-- IDX 6-->
<axis name="AILERONS" group ="REAL" failsafe_value="0"/> <!-- IDX 7-->
<axis name="FLAPS" group ="REAL" failsafe_value="0"/> <!-- IDX 8-->
<!-- Virtual actuators commands -->
<axis name="ROLL" group ="VIRTUAL" failsafe_value="0"/> <!-- ID X 9-->
<axis name="PITCH" group ="VIRTUAL" failsafe_value="0"/> <!-- IDX 10-->
<!-- Passive actuators commands -->
<axis name="ROT_MECH" group ="PASSIVE" failsafe_value="0"/> <!-- IDX 11-->
<!-- Legacy commands-->
<axis name="YAW" group ="OTHER" failsafe_value="0"/>
<axis name="THRUST" group ="OTHER" failsafe_value="0"/>
</commands> </commands>
<auto_rc_commands>
<set VALUE="@THROTTLE" COMMAND="THRUST"/>
<set VALUE="@ROLL" COMMAND="ROLL"/>
<set VALUE="@PITCH" COMMAND="PITCH"/>
<set VALUE="@YAW" COMMAND="YAW"/>
<set VALUE="@AUX4" COMMAND="THRUST_X"/>
</auto_rc_commands>
<command_laws> <command_laws>
<let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/> <let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[0])" SERVO="MOTOR_FRONT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])" SERVO="MOTOR_FRONT"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[1])" SERVO="MOTOR_RIGHT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])" SERVO="MOTOR_RIGHT"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[2])" SERVO="MOTOR_BACK"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )" SERVO="MOTOR_BACK"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[3])" SERVO="MOTOR_LEFT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )" SERVO="MOTOR_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : actuators_pprz[5])" SERVO="SERVO_RUDDER"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_PUSHER])" SERVO="MOTOR_PUSH"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[6]))" SERVO="SERVO_ELEVATOR"/> <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : commands[COMMAND_ELEVATOR]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[4])" SERVO="MOTOR_PUSH"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : commands[COMMAND_RUDDER])" SERVO="SERVO_RUDDER"/>
<!-- <set VALUE="rotwing_state_skewing.servo_pprz_cmd" SERVO="ROTATION_MECH"/> --> <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_LEFT"/>
<set VALUE="-9600" SERVO="ROTATION_MECH"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_RIGHT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="AIL_LEFT"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])" SERVO="FLAP_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="AIL_RIGHT"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])" SERVO="FLAP_RIGHT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[8])" SERVO="FLAP_LEFT"/> <set VALUE="commands[COMMAND_ROT_MECH]" SERVO="ROTATION_MECH"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[8])" SERVO="FLAP_RIGHT"/>
<!-- Backup commands --> <!-- Backup commands -->
<set VALUE="($th_hold? -9600 : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])" SERVO="BMOTOR_FRONT"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[2])" SERVO="BMOTOR_BACK"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )" SERVO="BMOTOR_BACK"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )" SERVO="BMOTOR_LEFT"/>
</command_laws> </command_laws>
<section NAME="MISC"> <section NAME="MISC">
@@ -223,8 +232,9 @@
<define name="NAV_CLIMB_VSPEED" value="2.0" /> <define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/> <define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="15"/> <define name="NAV_CARROT_DIST" value="15"/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="CLOSE_TO_WAYPOINT" value="15"/> <define name="CLOSE_TO_WAYPOINT" value="15"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="500"/> <define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/> <define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/> <define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/> <define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -233,37 +243,40 @@
<define name="STABILIZATION_ATTITUDE_SP_MAX_THETA" value="45." unit="deg" /> <define name="STABILIZATION_ATTITUDE_SP_MAX_THETA" value="45." unit="deg" />
<define name="STABILIZATION_ATTITUDE_SP_MAX_R" value="90." unit="deg/s"/> <define name="STABILIZATION_ATTITUDE_SP_MAX_R" value="90." unit="deg/s"/>
<define name="STABILIZATION_ATTITUDE_DEADBAND_R" value="200" /> <define name="STABILIZATION_ATTITUDE_DEADBAND_R" value="200" />
<define name="FWD_SIDESLIP_GAIN" value="2.0"/> <define name="FWD_SIDESLIP_GAIN" value="0.25"/> <!-- cyfoam 0.32-->
<!-- <define name="THRESHOLD_GROUND_DETECT" value="40"/> -->
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
</section> </section>
<section name="GROUND_DETECT"> <section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/> <define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/> <define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/> <define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/> <define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/> <define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/> <define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/> <define name="AGL_DIST_FILTER" value="0.07"/>
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-12,-9,20}, .scale={{30726,37910,15728},{3133,3871,1611}}, .filter_sample_freq=1134.1, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-9,-30,26}, .scale={{45288,818,33359},{8935,167,6832}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-35,-5,13}, .scale={{26152,56165,62837},{5357,11479,12884}}}}"/> <define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-12,-9,20}, .scale={{30726,37910,15728},{3133,3871,1611}}, .filter_sample_freq=1134.1, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-9,-30,26}, .scale={{45288,818,33359},{8935,167,6832}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-35,-5,13}, .scale={{26152,56165,62837},{5357,11479,12884}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-14,3,42}, .scale={{17279,2209,36874},{30247,3800,64095}}}}"/> <define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={785,-2641,40}, .scale={{7727,22807,32483},{13208,38799,54820}}}}"/>
<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1134.1, .filter_freq=30}}"/> <define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1134.1, .filter_freq=30}}"/>
<define name="BODY_TO_IMU_PHI" value="-2.6" unit="deg"/> <define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/> <define name="BODY_TO_IMU_THETA" value="0" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/> <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
<!-- <define name="MAG_X_NEUTRAL" value="785"/>
<define name="MAG_Y_NEUTRAL" value="-2641"/>
<define name="MAG_Z_NEUTRAL" value="40"/>
<define name="MAG_X_SENS" value="0.5850242272917106" integer="16"/>
<define name="MAG_Y_SENS" value="0.5878244286863009" integer="16"/>
<define name="MAG_Z_SENS" value="0.5925392191738064" integer="16"/> -->
</section> </section>
<section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI"> <section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI">
<define name = "G1_ROLL" value = "{ 0.0f, -13.0f, 0.0f, 13.0f, 0.0f, 0.0f, 0.0f}"/>
<define name = "G1_PITCH" value = "{ 1.6f, 0.0f, -1.6f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
<define name = "G1_YAW" value = "{-0.4f, 0.4f, -0.4f, 0.4f, 0.0f, 0.0f, 0.0f}"/>
<define name = "G1_THRUST" value = "{-0.575f, -0.575f, -0.575f, -0.575f, 0.55f, 0.0f, 0.0f}"/>
<define name = "G1_ZERO" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
<define name = "G2" value = "{-0.02f, 0.02f, -0.02f, 0.02f, 0.0f, 0.0f, 0.0f}"/>
<define name = "MAX_R" value = "120.0" unit="deg/s"/> <define name = "MAX_R" value = "120.0" unit="deg/s"/>
<define name = "FILT_CUTOFF" value = "3.0" /> <define name = "FILT_CUTOFF" value = "3.0" />
<define name = "FILT_CUTOFF_ACC" value = "5.0"/> <define name = "FILT_CUTOFF_ACC" value = "3.0"/>
<define name = "FILT_CUTOFF_VEL" value = "5.0"/> <define name = "FILT_CUTOFF_VEL" value = "5.0"/>
<define name = "FILT_CUTOFF_POS" value = "10.0"/> <define name = "FILT_CUTOFF_POS" value = "10.0"/>
<define name = "ESTIMATION_FILT_CUTOFF" value = "3.2" /> <define name = "ESTIMATION_FILT_CUTOFF" value = "3.2" />
@@ -272,18 +285,30 @@
<define name = "FILT_CUTOFF_R" value = "3.0" /> <define name = "FILT_CUTOFF_R" value = "3.0" />
<define name = "FILT_CUTOFF_RDOT" value = "0.5" /> <define name = "FILT_CUTOFF_RDOT" value = "0.5" />
<define name = "FILTER_YAW_RATE" value = "TRUE"/> <define name = "FILTER_YAW_RATE" value = "TRUE"/>
<define name = "ACT_DYN" value = "{10.1f, 10.1f, 10.1f, 10.1f, 24.07f, 0.0f,0.0f}" /> <!-- | MF | MR | MB | ML | MP | ELE | RUD | AIL | FLA | PHI | THETA | -->
<define name = "ACT_IS_SERVO" value = "{0, 0, 0, 0, 0, 0, 0}" /> <define name = "ACT_DYN" value = "{ 15.1f, 15.1f, 15.1f, 15.1f, 24.07f, 50.00f, 50.00f, 50.00f, 50.00f, 0.00f, 0.00f }" />
<define name = "ACT_MAX" value = "{9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, M_PI_4, M_PI_4}"/> <define name = "ACT_IS_SERVO" value = "{ 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0 }" />
<define name = "ACT_MIN" value = "{0.0f , 0.0f, 0.0f, 0.0f, 0.0f, -M_PI_4, -M_PI_4}"/> <define name = "ACT_MAX" value = "{ 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, M_PI_4, M_PI_4}"/>
<define name = "ACT_MAX_NORM" value = "{1.0f , 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}"/> <define name = "ACT_MIN" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -9600.0f, -9600.0f, -9600.0f, -M_PI_4, -M_PI_4}"/>
<define name = "ACT_MIN_NORM" value = "{0.0f , 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, -1.0f}"/> <define name = "ACT_MAX_NORM" value = "{ 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}"/>
<define name = "DEBUG_MODE" value = "FALSE"/> <define name = "ACT_MIN_NORM" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f}"/>
<define name = "AC_HAS_PUSHER" value = "TRUE"/> <define name = "WU" value = "{ 0.75f, 0.75f, 0.75f, 0.75f, 1.00f, 6.0f, 6.0f, 6.0f, 6.0f, 1.20f, 1.20f}"/>
<define name = "PUSHER_IDX" value = "4"/> <define name = "U_PREF" value = "{ 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 8000.0f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f}"/>
<define name = "WV" value = "{1.0f, 1.0f, 1.0f, 1000.0f, 1000.0f, 100.0f}"/> <define name = "WV" value = "{ 4.0f, 4.0f, 4.0f, 8.0f, 8.0f, 1.00f}"/>
<define name = "WU" value = "{0.75f, 0.75f, 0.75f, 0.75f, 1.0f, 2.0f, 2.0f}"/> </section>
<define name = "U_PREF" value = "{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
<section name="CTRL_EFF_SHED" prefix="ROT_WING_EFF_SCHED_">
<!-- | MF | MR | MB | ML | MP | ELE | RUD | AIL | FLA | PHI | THETA | -->
<!-- <define name = "dFdu" value = "{-0.50f, -0.50f, -0.50f, -0.50f, 0.55f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "dMzdu" value = "{-0.40f, 0.40f, -0.40f, 0.40f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "dMzdu_G2" value = "{-0.02f, 0.02f, -0.02f, 0.02f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "l" value = "{ 0.423f, 0.408f, 0.423f, 0.408f, 0.00f, 1.00f, 1.00f, 1.00f, 1.00f, 0.00f, 0.00f }"/> -->
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
</section> </section>
<section name="AUTOPILOT"> <section name="AUTOPILOT">
@@ -291,7 +316,7 @@
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/> <define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/> <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/> <define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="TRUE"/> <!-- <define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="TRUE"/> -->
</section> </section>
<section name="BAT"> <section name="BAT">
@@ -304,11 +329,9 @@
</section> </section>
<section name="SIMULATOR" prefix="NPS_"> <section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, pusher" type="string[]"/>
<define name="JSBSIM_MODEL" value="rotwingv3c_SI" type="string"/> <define name="JSBSIM_MODEL" value="rotwingv3c_SI" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/> <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="5"/> <define name="USE_COMMANDS" value="TRUE"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/> <define name="JS_AXIS_MODE" value="4"/>
</section> </section>
</airframe> </airframe>
conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml
+338
View File
@@ -0,0 +1,338 @@
<!-- This is a 7kg Rotating Wing Drone C
* Airframe: TUD00???
* Autopilot: Cube orange
* Datalink: Herelink
* GPS: UBlox F9P
* RC: SBUS Crossfire
-->
<airframe name="RotatingWingV3C">
<description>RotatingWingV3C with optitrack and INS EKF2</description>
<firmware name="rotorcraft">
<autopilot name="rotorcraft_oneloop_switch.xml"/>
<target name="ap" board="cube_orange">
<configure name="PERIODIC_FREQUENCY" value="500"/> <!-- Configure the main periodic frequency to 500Hz -->
<configure name="NAVIGATION_FREQUENCY" value="500"/>
<module name="radio_control" type="sbus">
<configure name="SBUS_PORT" value="UART3"/> <!-- On the TELEM2 port -->
</module>
<module name="sys_mon"/>
<module name="flight_recorder"/>
<module name="lidar_tfmini">
<configure name="TFMINI_PORT" value="UART8"/>
<configure name="USE_TFMINI_AGL" value="TRUE"/>
</module>
<!-- RC switches -->
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="RADIO_FMODE" value="RADIO_AUX2"/>
<define name="AP_MODE_SWITCH" value="RADIO_AUX3"/>
<define name="RADIO_CONTROL_THRUST_X" value="RADIO_AUX4"/>
<!-- EKF EKF2 configure inputs -->
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="INS_EKF2_IMU_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_GYRO_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<!--Only send gyro and accel that is being used-->
<define name="IMU_GYRO_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<!--Use adc rot sensor-->
<module name="wing_rotation_adc_sensor">
<define name="ADC_WING_ROT_OFFSET" value="-49.1731"/>
<define name="ADC_WING_ROT_SCALE" value="0.0029986"/>
</module>
<!-- Log in high speed (Remove for outdoor flights) -->
<!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> -->
<define name="I2C2_CLOCK_SPEED" value="100000"/>
</target>
<target name="nps" board="pc">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<configure name="NAVIGATION_FREQUENCY" value="500"/>
<module name="radio_control" type="datalink"/>
<module name="fdm" type="jsbsim"/>
<!--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="AP_MODE_SWITCH" value="RADIO_AUX7"/> <!-- Switch between AP controllers -->
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/> <!-- Kill switch -->
<define name="RADIO_CONTROL_THRUST_X" value="0"/>
</target>
<!-- Datalink -->
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/> <!-- herelink-->
<!-- <configure VALUE="B57600" name="MODEM_BAUD"/> xBee -->
</module>
<!-- Sensors -->
<module name="mag" type="rm3100">
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_RM3100_I2C_DEV" value="I2C2"/>
</module>
<module name="airspeed" type="ms45xx_i2c">
<define name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.9077609"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/>
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
</module>
<module name="airspeed" type="uavcan">
<define name="AIRSPEED_UAVCAN_LOWPASS_FILTER" value="TRUE" />
<define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD" value="0.1" />
<define name="AIRSPEED_UAVCAN_LOWPASS_TAU" value="0.15" />
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module>
<module name="air_data"/>
<module name="gps" type="datalink"/>
<!-- IMU / INS -->
<module name="imu" type="cube"/>
<module name="ins" type="ekf2">
<define name="INS_EKF2_OPTITRACK" value="true"/>
<define name="INS_EKF2_RW" value="true"/>
</module>
<!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan">
<configure value="TRUE" name="UAVCAN_USE_CAN1"/>
<configure value="TRUE" name="UAVCAN_USE_CAN2"/>
</module>
<!-- Actuators on PWM -->
<module name="actuators" type="pwm" >
<define name="SERVO_HZ" value="400"/>
</module>
<!-- Control -->
<module name="stabilization" type="oneloop" >
<configure name="INDI_NUM_ACT" value="9"/>
</module>
<module name="guidance" type="oneloop"/>
<module name="nav" type="hybrid">
<define name="NAV_HYBRID_MAX_AIRSPEED" value="19.0f"/>
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
<define name="NAV_HYBRID_MAX_ACCELERATION" value="4.0f"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
<define name="NAV_OPTITRACK" value="TRUE"/>
</module>
<module name="oneloop" type="andi">
<configure name="ANDI_OUTPUTS" value="6"/>
</module>
<module name="eff_scheduling_rot_wing_V2"/>
<module name="wls">
<define name="WLS_N_U" value = "11"/>
<define name="WLS_N_V" value = "6"/>
</module>
<module name="ground_detect_sensor"/>
<module name="rotwing_state_V2"/>
<module name="agl_dist"/>
<module name="rot_wing_automation">
<define name="USE_V2" value="TRUE"/>
</module>
</firmware>
<!-- PWM actuators -->
<servos driver="Pwm">
<servo no="0" name="ROTATION_MECH" min="1360" neutral="1586" max="1812"/>
</servos>
<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="600" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="600" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="600" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="600" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="200" max="8191"/>
</servos>
<!-- CAN BUS 1 command outputs-->
<servos driver="Uavcan1Cmd">
<servo no="5" name="SERVO_ELEVATOR" min="5737" neutral="5737" max="-3936"/>
<servo no="6" name="SERVO_RUDDER" min="-5225" neutral="0" max="5225"/>
</servos>
<!-- Can bus 2 actuators -->
<servos driver="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="600" max="8191"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="600" max="8191"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="600" max="8191"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="600" max="8191"/>
</servos>
<!-- CAN BUS 2 command outputs-->
<servos driver="Uavcan2Cmd">
<servo no="7" name="AIL_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
<servo no="8" name="FLAP_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
<servo no="9" name="FLAP_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
<servo no="10" name="AIL_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
</servos>
<commands>
<!-- Real actuators commands -->
<axis name="MOTOR_FRONT" group ="REAL" failsafe_value="0"/> <!-- IDX 0-->
<axis name="MOTOR_RIGHT" group ="REAL" failsafe_value="0"/> <!-- IDX 1-->
<axis name="MOTOR_BACK" group ="REAL" failsafe_value="0"/> <!-- IDX 2-->
<axis name="MOTOR_LEFT" group ="REAL" failsafe_value="0"/> <!-- IDX 3-->
<axis name="MOTOR_PUSHER" group ="REAL" failsafe_value="0"/> <!-- IDX 4-->
<axis name="ELEVATOR" group ="REAL" failsafe_value="0"/> <!-- IDX 5-->
<axis name="RUDDER" group ="REAL" failsafe_value="0"/> <!-- IDX 6-->
<axis name="AILERONS" group ="REAL" failsafe_value="0"/> <!-- IDX 7-->
<axis name="FLAPS" group ="REAL" failsafe_value="0"/> <!-- IDX 8-->
<!-- Virtual actuators commands -->
<axis name="ROLL" group ="VIRTUAL" failsafe_value="0"/> <!-- ID X 9-->
<axis name="PITCH" group ="VIRTUAL" failsafe_value="0"/> <!-- IDX 10-->
<!-- Passive actuators commands -->
<axis name="ROT_MECH" group ="PASSIVE" failsafe_value="0"/> <!-- IDX 11-->
<!-- Legacy commands-->
<axis name="YAW" group ="OTHER" failsafe_value="0"/>
<axis name="THRUST" group ="OTHER" failsafe_value="0"/>
</commands>
<command_laws>
<let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])" SERVO="MOTOR_FRONT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])" SERVO="MOTOR_RIGHT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )" SERVO="MOTOR_BACK"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )" SERVO="MOTOR_LEFT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_PUSHER])" SERVO="MOTOR_PUSH"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : commands[COMMAND_ELEVATOR]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : commands[COMMAND_RUDDER])" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_RIGHT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])" SERVO="FLAP_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])" SERVO="FLAP_RIGHT"/>
<set VALUE="commands[COMMAND_ROT_MECH]" SERVO="ROTATION_MECH"/>
<!-- Backup commands -->
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])" SERVO="BMOTOR_FRONT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )" SERVO="BMOTOR_BACK"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )" SERVO="BMOTOR_LEFT"/>
</command_laws>
<section NAME="MISC">
<!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
<!-- Others -->
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="15"/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="CLOSE_TO_WAYPOINT" value="15"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="STABILIZATION_ATTITUDE_SP_MAX_PHI" value="45." unit="deg" />
<define name="STABILIZATION_ATTITUDE_SP_MAX_THETA" value="45." unit="deg" />
<define name="STABILIZATION_ATTITUDE_SP_MAX_R" value="90." unit="deg/s"/>
<define name="STABILIZATION_ATTITUDE_DEADBAND_R" value="200" />
<define name="FWD_SIDESLIP_GAIN" value="0.25"/> <!-- cyfoam 0.32-->
<!-- <define name="THRESHOLD_GROUND_DETECT" value="40"/> -->
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
<section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-12,-9,20}, .scale={{30726,37910,15728},{3133,3871,1611}}, .filter_sample_freq=1134.1, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-9,-30,26}, .scale={{45288,818,33359},{8935,167,6832}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-35,-5,13}, .scale={{26152,56165,62837},{5357,11479,12884}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={785,-2641,40}, .scale={{7727,22807,32483},{13208,38799,54820}}}}"/>
<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1134.1, .filter_freq=30}}"/>
<define name="BODY_TO_IMU_PHI" value="0" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0" unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
<!-- <define name="MAG_X_NEUTRAL" value="785"/>
<define name="MAG_Y_NEUTRAL" value="-2641"/>
<define name="MAG_Z_NEUTRAL" value="40"/>
<define name="MAG_X_SENS" value="0.5850242272917106" integer="16"/>
<define name="MAG_Y_SENS" value="0.5878244286863009" integer="16"/>
<define name="MAG_Z_SENS" value="0.5925392191738064" integer="16"/> -->
</section>
<section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI">
<define name = "MAX_R" value = "120.0" unit="deg/s"/>
<define name = "FILT_CUTOFF" value = "3.0" />
<define name = "FILT_CUTOFF_ACC" value = "3.0"/>
<define name = "FILT_CUTOFF_VEL" value = "5.0"/>
<define name = "FILT_CUTOFF_POS" value = "10.0"/>
<define name = "ESTIMATION_FILT_CUTOFF" value = "3.2" />
<define name = "FILT_CUTOFF_P" value = "3.0"/>
<define name = "FILT_CUTOFF_Q" value = "3.0"/>
<define name = "FILT_CUTOFF_R" value = "3.0" />
<define name = "FILT_CUTOFF_RDOT" value = "0.5" />
<define name = "FILTER_YAW_RATE" value = "TRUE"/>
<!-- | MF | MR | MB | ML | MP | ELE | RUD | AIL | FLA | PHI | THETA | -->
<define name = "ACT_DYN" value = "{ 15.1f, 15.1f, 15.1f, 15.1f, 24.07f, 50.00f, 50.00f, 50.00f, 50.00f, 0.00f, 0.00f }" />
<define name = "ACT_IS_SERVO" value = "{ 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0 }" />
<define name = "ACT_MAX" value = "{ 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, M_PI_4, M_PI_4}"/>
<define name = "ACT_MIN" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -9600.0f, -9600.0f, -9600.0f, -M_PI_4, -M_PI_4}"/>
<define name = "ACT_MAX_NORM" value = "{ 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}"/>
<define name = "ACT_MIN_NORM" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f}"/>
<define name = "WU" value = "{ 0.75f, 0.75f, 0.75f, 0.75f, 1.00f, 6.0f, 6.0f, 6.0f, 6.0f, 1.20f, 1.20f}"/>
<define name = "U_PREF" value = "{ 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 8000.0f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f}"/>
<define name = "WV" value = "{ 4.0f, 4.0f, 4.0f, 8.0f, 8.0f, 1.00f}"/>
</section>
<section name="CTRL_EFF_SHED" prefix="ROT_WING_EFF_SCHED_">
<!-- | MF | MR | MB | ML | MP | ELE | RUD | AIL | FLA | PHI | THETA | -->
<!-- <define name = "dFdu" value = "{-0.50f, -0.50f, -0.50f, -0.50f, 0.55f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "dMzdu" value = "{-0.40f, 0.40f, -0.40f, 0.40f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "dMzdu_G2" value = "{-0.02f, 0.02f, -0.02f, 0.02f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "l" value = "{ 0.423f, 0.408f, 0.423f, 0.408f, 0.00f, 1.00f, 1.00f, 1.00f, 1.00f, 0.00f, 0.00f }"/> -->
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<!-- <define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="TRUE"/> -->
</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="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="JSBSIM_MODEL" value="rotwingv3c_SI" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="USE_COMMANDS" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
</airframe>
conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
+104 -75
View File
@@ -10,7 +10,7 @@
<description>RotatingWingV3C with optitrack and INS ext pose</description> <description>RotatingWingV3C with optitrack and INS ext pose</description>
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<autopilot name="rotorcraft_oneloop_andi_indi.xml"/> <autopilot name="rotorcraft_oneloop_switch.xml"/>
<target name="ap" board="cube_orange"> <target name="ap" board="cube_orange">
<configure name="PERIODIC_FREQUENCY" value="500"/> <!-- Configure the main periodic frequency to 500Hz --> <configure name="PERIODIC_FREQUENCY" value="500"/> <!-- Configure the main periodic frequency to 500Hz -->
<configure name="NAVIGATION_FREQUENCY" value="500"/> <configure name="NAVIGATION_FREQUENCY" value="500"/>
@@ -28,7 +28,7 @@
<define name="RADIO_TH_HOLD" value="RADIO_AUX1"/> <define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/> <define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
<define name="RADIO_FMODE" value="RADIO_AUX2"/> <define name="RADIO_FMODE" value="RADIO_AUX2"/>
<define name="RADIO_FBW_MODE" value="RADIO_AUX3"/> <define name="AP_MODE_SWITCH" value="RADIO_AUX3"/>
<define name="RADIO_CONTROL_THRUST_X" value="RADIO_AUX4"/> <define name="RADIO_CONTROL_THRUST_X" value="RADIO_AUX4"/>
<!-- EKF EXT POSE configure inputs --> <!-- EKF EXT POSE configure inputs -->
<define name="INS_EXT_POSE_MAG_ID" value="MAG_RM3100_SENDER_ID"/> <define name="INS_EXT_POSE_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
@@ -38,8 +38,8 @@
<define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/> <define name="IMU_ACCEL_ABI_SEND_ID" value= "IMU_CUBE1_ID"/>
<!--Use adc rot sensor--> <!--Use adc rot sensor-->
<module name="wing_rotation_adc_sensor"> <module name="wing_rotation_adc_sensor">
<define name="ADC_OFFSET" value="-49.1731"/> <define name="ADC_WING_ROT_OFFSET" value="-49.1731"/>
<define name="ADC_SCALE" value="0.0029986"/> <define name="ADC_WING_ROT_SCALE" value="0.0029986"/>
</module> </module>
<!-- Log in high speed (Remove for outdoor flights) --> <!-- Log in high speed (Remove for outdoor flights) -->
<!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> --> <!-- <define name="IMU_LOG_HIGHSPEED" value="TRUE"/> -->
@@ -55,8 +55,8 @@
<!--Not dealing with these in the simulation--> <!--Not dealing with these in the simulation-->
<define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws --> <define name="RADIO_TH_HOLD" value="0"/> <!-- Throttle hold in command laws -->
<define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select --> <define name="RADIO_FMODE" value="0"/> <!-- Throttle curve select -->
<define name="RADIO_FBW_MODE" value="0"/> <!-- Switch between AP and FBW control --> <define name="AP_MODE_SWITCH" value="RADIO_AUX7"/> <!-- Switch between AP controllers -->
<define name="RADIO_KILL_SWITCH" value="5"/> <!-- Kill switch --> <define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/> <!-- Kill switch -->
<define name="RADIO_CONTROL_THRUST_X" value="0"/> <define name="RADIO_CONTROL_THRUST_X" value="0"/>
</target> </target>
@@ -109,32 +109,31 @@
<module name="guidance" type="oneloop"/> <module name="guidance" type="oneloop"/>
<module name="nav" type="hybrid"> <module name="nav" type="hybrid">
<define name="NAV_HYBRID_MAX_AIRSPEED" value="5.0f"/> <define name="NAV_HYBRID_MAX_AIRSPEED" value="19.0f"/>
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/> <define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
<define name="NAV_HYBRID_MAX_ACCELERATION" value="4.0f"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/> <define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
<define name="GUIDANCE_H_USE_REF" value="FALSE"/> <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
<define name="NAV_HYBRID_GOTO_MODE" value="NAV_SETPOINT_MODE_POS"/> <define name="NAV_OPTITRACK" value="TRUE"/>
</module> </module>
<module name="oneloop" type="andi"> <module name="oneloop" type="andi">
<configure name="ANDI_NUM_ACT_TOT" value="7"/>
<configure name="ANDI_NUM_ACT" value="5"/>
<configure name="ANDI_NUM_VIRTUAL_ACT" value="2"/>
<configure name="ANDI_OUTPUTS" value="6"/> <configure name="ANDI_OUTPUTS" value="6"/>
</module> </module>
<module name="eff_scheduling_rot_wing_V2"/>
<module name="wls"> <module name="wls">
<define name="WLS_N_U" value = "7"/> <define name="WLS_N_U" value = "11"/>
<define name="WLS_N_V" value = "6"/> <define name="WLS_N_V" value = "6"/>
</module> </module>
<module name="ground_detect_sensor"/> <module name="ground_detect_sensor"/>
<!-- <module name="rotwing_state"> <module name="rotwing_state_V2"/>
<define name = "ROT_MECH_IDX" value = "9"/>
<define name = "USE_ROTMECH_VIRTUAL" value = "FALSE"/>
<define name = "ROTMECH_DYN" value = "3.0"/>
</module> -->
<module name="agl_dist"/> <module name="agl_dist"/>
<module name="rot_wing_automation">
<define name="USE_V2" value="TRUE"/>
</module>
</firmware> </firmware>
<!-- PWM actuators --> <!-- PWM actuators -->
@@ -153,7 +152,7 @@
<!-- CAN BUS 1 command outputs--> <!-- CAN BUS 1 command outputs-->
<servos driver="Uavcan1Cmd"> <servos driver="Uavcan1Cmd">
<servo no="5" name="SERVO_ELEVATOR" min="3980" neutral="3980" max="-5280"/> <servo no="5" name="SERVO_ELEVATOR" min="5737" neutral="5737" max="-3936"/>
<servo no="6" name="SERVO_RUDDER" min="-5225" neutral="0" max="5225"/> <servo no="6" name="SERVO_RUDDER" min="-5225" neutral="0" max="5225"/>
</servos> </servos>
@@ -174,42 +173,47 @@
</servos> </servos>
<commands> <commands>
<axis NAME="ROLL" FAILSAFE_VALUE="0"/> <!-- Real actuators commands -->
<axis NAME="PITCH" FAILSAFE_VALUE="0"/> <axis name="MOTOR_FRONT" group ="REAL" failsafe_value="0"/> <!-- IDX 0-->
<axis NAME="YAW" FAILSAFE_VALUE="0"/> <axis name="MOTOR_RIGHT" group ="REAL" failsafe_value="0"/> <!-- IDX 1-->
<axis NAME="THRUST" FAILSAFE_VALUE="0"/> <axis name="MOTOR_BACK" group ="REAL" failsafe_value="0"/> <!-- IDX 2-->
<axis name="MOTOR_LEFT" group ="REAL" failsafe_value="0"/> <!-- IDX 3-->
<axis name="MOTOR_PUSHER" group ="REAL" failsafe_value="0"/> <!-- IDX 4-->
<axis name="ELEVATOR" group ="REAL" failsafe_value="0"/> <!-- IDX 5-->
<axis name="RUDDER" group ="REAL" failsafe_value="0"/> <!-- IDX 6-->
<axis name="AILERONS" group ="REAL" failsafe_value="0"/> <!-- IDX 7-->
<axis name="FLAPS" group ="REAL" failsafe_value="0"/> <!-- IDX 8-->
<!-- Virtual actuators commands -->
<axis name="ROLL" group ="VIRTUAL" failsafe_value="0"/> <!-- ID X 9-->
<axis name="PITCH" group ="VIRTUAL" failsafe_value="0"/> <!-- IDX 10-->
<!-- Passive actuators commands -->
<axis name="ROT_MECH" group ="PASSIVE" failsafe_value="0"/> <!-- IDX 11-->
<!-- Legacy commands-->
<axis name="YAW" group ="OTHER" failsafe_value="0"/>
<axis name="THRUST" group ="OTHER" failsafe_value="0"/>
</commands> </commands>
<auto_rc_commands>
<set VALUE="@THROTTLE" COMMAND="THRUST"/>
<set VALUE="@ROLL" COMMAND="ROLL"/>
<set VALUE="@PITCH" COMMAND="PITCH"/>
<set VALUE="@YAW" COMMAND="YAW"/>
<set VALUE="@AUX4" COMMAND="THRUST_X"/>
</auto_rc_commands>
<command_laws> <command_laws>
<let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/> <let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/> <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[0])" SERVO="MOTOR_FRONT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])" SERVO="MOTOR_FRONT"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[1])" SERVO="MOTOR_RIGHT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])" SERVO="MOTOR_RIGHT"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[2])" SERVO="MOTOR_BACK"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )" SERVO="MOTOR_BACK"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[3])" SERVO="MOTOR_LEFT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )" SERVO="MOTOR_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : actuators_pprz[5])" SERVO="SERVO_RUDDER"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_PUSHER])" SERVO="MOTOR_PUSH"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[6]))" SERVO="SERVO_ELEVATOR"/> <set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : commands[COMMAND_ELEVATOR]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[4])" SERVO="MOTOR_PUSH"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : commands[COMMAND_RUDDER])" SERVO="SERVO_RUDDER"/>
<!-- <set VALUE="rotwing_state_skewing.servo_pprz_cmd" SERVO="ROTATION_MECH"/> --> <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_LEFT"/>
<set VALUE="-9600" SERVO="ROTATION_MECH"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_RIGHT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="AIL_LEFT"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])" SERVO="FLAP_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="AIL_RIGHT"/> <set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])" SERVO="FLAP_RIGHT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[8])" SERVO="FLAP_LEFT"/> <set VALUE="commands[COMMAND_ROT_MECH]" SERVO="ROTATION_MECH"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[8])" SERVO="FLAP_RIGHT"/>
<!-- Backup commands --> <!-- Backup commands -->
<set VALUE="($th_hold? -9600 : actuators_pprz[0])" SERVO="BMOTOR_FRONT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])" SERVO="BMOTOR_FRONT"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[1])" SERVO="BMOTOR_RIGHT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[2])" SERVO="BMOTOR_BACK"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )" SERVO="BMOTOR_BACK"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[3])" SERVO="BMOTOR_LEFT"/> <set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )" SERVO="BMOTOR_LEFT"/>
</command_laws> </command_laws>
<section NAME="MISC"> <section NAME="MISC">
@@ -221,8 +225,9 @@
<define name="NAV_CLIMB_VSPEED" value="2.0" /> <define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/> <define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="15"/> <define name="NAV_CARROT_DIST" value="15"/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="CLOSE_TO_WAYPOINT" value="15"/> <define name="CLOSE_TO_WAYPOINT" value="15"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="500"/> <define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/> <define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/> <define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/> <define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -231,23 +236,24 @@
<define name="STABILIZATION_ATTITUDE_SP_MAX_THETA" value="45." unit="deg" /> <define name="STABILIZATION_ATTITUDE_SP_MAX_THETA" value="45." unit="deg" />
<define name="STABILIZATION_ATTITUDE_SP_MAX_R" value="90." unit="deg/s"/> <define name="STABILIZATION_ATTITUDE_SP_MAX_R" value="90." unit="deg/s"/>
<define name="STABILIZATION_ATTITUDE_DEADBAND_R" value="200" /> <define name="STABILIZATION_ATTITUDE_DEADBAND_R" value="200" />
<define name="FWD_SIDESLIP_GAIN" value="2.0"/> <define name="FWD_SIDESLIP_GAIN" value="0.25"/> <!-- cyfoam 0.32-->
<!-- <define name="THRESHOLD_GROUND_DETECT" value="40"/> --> <!-- <define name="THRESHOLD_GROUND_DETECT" value="40"/> -->
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/> <define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
<define name="INS_EXT_POSE_RW" value="TRUE"/>
</section> </section>
<section name="GROUND_DETECT"> <section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/> <define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/> <define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/> <define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/> <define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.18"/> <define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/> <define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/> <define name="AGL_DIST_FILTER" value="0.07"/>
</section> </section>
<section name="IMU" prefix="IMU_"> <section name="IMU" prefix="IMU_">
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-12,-9,20}, .scale={{30726,37910,15728},{3133,3871,1611}}, .filter_sample_freq=1134.1, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-9,-30,26}, .scale={{45288,818,33359},{8935,167,6832}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-35,-5,13}, .scale={{26152,56165,62837},{5357,11479,12884}}}}"/> <define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-12,-9,20}, .scale={{30726,37910,15728},{3133,3871,1611}}, .filter_sample_freq=1134.1, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-9,-30,26}, .scale={{45288,818,33359},{8935,167,6832}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-35,-5,13}, .scale={{26152,56165,62837},{5357,11479,12884}}}}"/>
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={-14,3,42}, .scale={{17279,2209,36874},{30247,3800,64095}}}}"/> <define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={785,-2641,40}, .scale={{7727,22807,32483},{13208,38799,54820}}}}"/>
<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1134.1, .filter_freq=30}}"/> <define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1134.1, .filter_freq=30}}"/>
<define name="BODY_TO_IMU_PHI" value="-4.54" unit="deg"/> <define name="BODY_TO_IMU_PHI" value="-4.54" unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="1.84" unit="deg"/> <define name="BODY_TO_IMU_THETA" value="1.84" unit="deg"/>
@@ -255,15 +261,9 @@
</section> </section>
<section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI"> <section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI">
<define name = "G1_ROLL" value = "{ 0.0f, -13.0f, 0.0f, 13.0f, 0.0f, 0.0f, 0.0f}"/>
<define name = "G1_PITCH" value = "{ 1.6f, 0.0f, -1.6f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
<define name = "G1_YAW" value = "{-0.4f, 0.4f, -0.4f, 0.4f, 0.0f, 0.0f, 0.0f}"/>
<define name = "G1_THRUST" value = "{-0.575f, -0.575f, -0.575f, -0.575f, 0.52f, 0.0f, 0.0f}"/>
<define name = "G1_ZERO" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/>
<define name = "G2" value = "{-0.02f, 0.02f, -0.02f, 0.02f, 0.0f, 0.0f, 0.0f}"/>
<define name = "MAX_R" value = "120.0" unit="deg/s"/> <define name = "MAX_R" value = "120.0" unit="deg/s"/>
<define name = "FILT_CUTOFF" value = "3.0" /> <define name = "FILT_CUTOFF" value = "3.0" />
<define name = "FILT_CUTOFF_ACC" value = "5.0"/> <define name = "FILT_CUTOFF_ACC" value = "3.0"/>
<define name = "FILT_CUTOFF_VEL" value = "5.0"/> <define name = "FILT_CUTOFF_VEL" value = "5.0"/>
<define name = "FILT_CUTOFF_POS" value = "10.0"/> <define name = "FILT_CUTOFF_POS" value = "10.0"/>
<define name = "ESTIMATION_FILT_CUTOFF" value = "3.2" /> <define name = "ESTIMATION_FILT_CUTOFF" value = "3.2" />
@@ -272,19 +272,50 @@
<define name = "FILT_CUTOFF_R" value = "3.0" /> <define name = "FILT_CUTOFF_R" value = "3.0" />
<define name = "FILT_CUTOFF_RDOT" value = "0.5" /> <define name = "FILT_CUTOFF_RDOT" value = "0.5" />
<define name = "FILTER_YAW_RATE" value = "TRUE"/> <define name = "FILTER_YAW_RATE" value = "TRUE"/>
<define name = "ACT_DYN" value = "{10.1f, 10.1f, 10.1f, 10.1f, 24.07f, 0.0f,0.0f}" /> <!-- | MF | MR | MB | ML | MP | ELE | RUD | AIL | FLA | PHI | THETA | -->
<define name = "ACT_IS_SERVO" value = "{0, 0, 0, 0, 0, 0, 0}" /> <define name = "ACT_DYN" value = "{ 15.1f, 15.1f, 15.1f, 15.1f, 24.07f, 50.00f, 50.00f, 50.00f, 50.00f, 0.00f, 0.00f }" />
<define name = "ACT_MAX" value = "{9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, M_PI_4, M_PI_4}"/> <define name = "ACT_IS_SERVO" value = "{ 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0 }" />
<define name = "ACT_MIN" value = "{0.0f , 0.0f, 0.0f, 0.0f, 0.0f, -M_PI_4, -M_PI_4}"/> <define name = "ACT_MAX" value = "{ 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, M_PI_4, M_PI_4}"/>
<define name = "ACT_MAX_NORM" value = "{1.0f , 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}"/> <define name = "ACT_MIN" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -9600.0f, -9600.0f, -9600.0f, -M_PI_4, -M_PI_4}"/>
<define name = "ACT_MIN_NORM" value = "{0.0f , 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, -1.0f}"/> <define name = "ACT_MAX_NORM" value = "{ 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}"/>
<define name = "DEBUG_MODE" value = "FALSE"/> <define name = "ACT_MIN_NORM" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f}"/>
<define name = "AC_HAS_PUSHER" value = "TRUE"/> <define name = "WU" value = "{ 0.75f, 0.75f, 0.75f, 0.75f, 1.00f, 6.0f, 6.0f, 6.0f, 6.0f, 1.20f, 1.20f}"/>
<define name = "PUSHER_IDX" value = "4"/> <define name = "U_PREF" value = "{ 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 8000.0f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f}"/>
<define name = "WV" value = "{1.0f, 1.0f, 1.0f, 1000.0f, 1000.0f, 100.0f}"/> <define name = "WV" value = "{ 4.0f, 4.0f, 4.0f, 8.0f, 8.0f, 1.00f}"/>
<define name = "WU" value = "{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 10.0f, 1.0f}"/> </section>
<!-- <define name = "WU" value = "{0.75f, 0.75f, 0.75f, 0.75f, 1.0f, 2.0f, 2.0f}"/> -->
<define name = "U_PREF" value = "{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}"/> <section name="CTRL_EFF_SHED" prefix="ROT_WING_EFF_SCHED_">
<!-- | MF | MR | MB | ML | MP | ELE | RUD | AIL | FLA | PHI | THETA | -->
<!-- <define name = "dFdu" value = "{-0.50f, -0.50f, -0.50f, -0.50f, 0.55f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "dMzdu" value = "{-0.40f, 0.40f, -0.40f, 0.40f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "dMzdu_G2" value = "{-0.02f, 0.02f, -0.02f, 0.02f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "l" value = "{ 0.423f, 0.408f, 0.423f, 0.408f, 0.00f, 1.00f, 1.00f, 1.00f, 1.00f, 0.00f, 0.00f }"/> -->
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section> </section>
<section name="AUTOPILOT"> <section name="AUTOPILOT">
@@ -305,11 +336,9 @@
</section> </section>
<section name="SIMULATOR" prefix="NPS_"> <section name="SIMULATOR" prefix="NPS_">
<define name="ACTUATOR_NAMES" value="front_motor, right_motor, back_motor, left_motor, pusher, rudder, elevator, aileron_left, aileron_right" type="string[]"/> <define name="JSBSIM_MODEL" value="rotwingv3c_SI" type="string"/>
<define name="JSBSIM_MODEL" value="rotwingv3c" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/> <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="COMMANDS_NB" value="9"/> <define name="USE_COMMANDS" value="TRUE"/>
<define name="NO_MOTOR_MIXING" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/> <define name="JS_AXIS_MODE" value="4"/>
</section> </section>
</airframe> </airframe>
conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml
+312
View File
@@ -0,0 +1,312 @@
<!-- This is a 7kg Rotating Wing Drone C
* Airframe: TUD00???
* Autopilot: Cube orange
* Datalink: Herelink
* GPS: UBlox F9P
* RC: SBUS Crossfire
-->
<airframe name="RotatingWingV3C">
<description>RotatingWingV3C for simulation</description>
<firmware name="rotorcraft">
<autopilot name="rotorcraft_oneloop_switch.xml"/>
<target name="nps" board="pc">
<configure name="PERIODIC_FREQUENCY" value="500"/>
<configure name="NAVIGATION_FREQUENCY" value="500"/>
<module name="radio_control" type="datalink"/>
<module name="fdm" type="jsbsim"/>
<!--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="AP_MODE_SWITCH" value="RADIO_AUX7"/> <!-- Switch between AP controllers -->
<define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/> <!-- Kill switch -->
<define name="RADIO_CONTROL_THRUST_X" value="0"/>
</target>
<!-- Datalink -->
<module name="telemetry" type="transparent">
<configure name="MODEM_BAUD" value="B115200"/> <!-- herelink-->
<!-- <configure VALUE="B57600" name="MODEM_BAUD"/> xBee -->
</module>
<!-- Sensors -->
<module name="mag" type="rm3100">
<define name="MODULE_RM3100_UPDATE_AHRS" value="TRUE"/>
<configure name="MAG_RM3100_I2C_DEV" value="I2C2"/>
</module>
<module name="airspeed" type="ms45xx_i2c">
<define name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.9077609"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.15"/>
<define name="AIRSPEED_MS45XX_SEND_ABI" value="1"/>
</module>
<module name="airspeed" type="uavcan">
<define name="AIRSPEED_UAVCAN_LOWPASS_FILTER" value="TRUE" />
<define name="AIRSPEED_UAVCAN_LOWPASS_PERIOD" value="0.1" />
<define name="AIRSPEED_UAVCAN_LOWPASS_TAU" value="0.15" />
<define name="AIRSPEED_UAVCAN_SEND_ABI" value="0" /> <!-- Read Airspeed for logging but do not use it -->
</module>
<module name="air_data"/>
<module name="gps" type="ublox">
<configure name="UBX_GPS_BAUD" value="B460800"/>
<define name="USE_GPS_UBX_RTCM" value="TRUE"/>
</module>
<!-- IMU / INS -->
<module name="imu" type="cube"/>
<module name="ins" type="ekf2"/>
<!-- Actuators on dual CAN bus -->
<module name="actuators" type="uavcan">
<configure value="TRUE" name="UAVCAN_USE_CAN1"/>
<configure value="TRUE" name="UAVCAN_USE_CAN2"/>
</module>
<!-- Actuators on PWM -->
<module name="actuators" type="pwm" >
<define name="SERVO_HZ" value="400"/>
</module>
<!-- Control -->
<module name="stabilization" type="oneloop" >
<configure name="INDI_NUM_ACT" value="9"/>
</module>
<module name="guidance" type="oneloop"/>
<module name="nav" type="hybrid">
<define name="NAV_HYBRID_MAX_AIRSPEED" value="19.0f"/>
<define name="NAV_HYBRID_MAX_SPEED_V" value="3.0f"/>
<define name="NAV_HYBRID_SPEED_MARGIN" value="0.0f"/>
<define name="NAV_HYBRID_MAX_ACCELERATION" value="4.0f"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
</module>
<module name="oneloop" type="andi">
<configure name="ANDI_OUTPUTS" value="6"/>
</module>
<module name="eff_scheduling_rot_wing_V2"/>
<module name="wls">
<define name="WLS_N_U" value = "11"/>
<define name="WLS_N_V" value = "6"/>
</module>
<module name="ground_detect_sensor"/>
<module name="rotwing_state_V2">
<define name = "USE_ROTMECH_VIRTUAL" value = "TRUE"/>
<define name = "ROTMECH_DYN" value = "3.0"/>
</module>
<module name="agl_dist"/>
<module name="rot_wing_automation">
<define name="USE_V2" value="TRUE"/>
</module>
</firmware>
<!-- PWM actuators -->
<servos driver="Pwm">
<servo no="0" name="ROTATION_MECH" min="1360" neutral="1586" max="1812"/>
</servos>
<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="600" max="8191"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="600" max="8191"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="600" max="8191"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="600" max="8191"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="200" max="8191"/>
</servos>
<!-- CAN BUS 1 command outputs-->
<servos driver="Uavcan1Cmd">
<servo no="5" name="SERVO_ELEVATOR" min="5737" neutral="5737" max="-3936"/>
<servo no="6" name="SERVO_RUDDER" min="-5225" neutral="0" max="5225"/>
</servos>
<!-- Can bus 2 actuators -->
<servos driver="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="600" max="8191"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="600" max="8191"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="600" max="8191"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="600" max="8191"/>
</servos>
<!-- CAN BUS 2 command outputs-->
<servos driver="Uavcan2Cmd">
<servo no="7" name="AIL_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
<servo no="8" name="FLAP_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
<servo no="9" name="FLAP_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
<servo no="10" name="AIL_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
</servos>
<commands>
<!-- Real actuators commands -->
<axis name="MOTOR_FRONT" group ="REAL" failsafe_value="0"/> <!-- IDX 0-->
<axis name="MOTOR_RIGHT" group ="REAL" failsafe_value="0"/> <!-- IDX 1-->
<axis name="MOTOR_BACK" group ="REAL" failsafe_value="0"/> <!-- IDX 2-->
<axis name="MOTOR_LEFT" group ="REAL" failsafe_value="0"/> <!-- IDX 3-->
<axis name="MOTOR_PUSHER" group ="REAL" failsafe_value="0"/> <!-- IDX 4-->
<axis name="ELEVATOR" group ="REAL" failsafe_value="0"/> <!-- IDX 5-->
<axis name="RUDDER" group ="REAL" failsafe_value="0"/> <!-- IDX 6-->
<axis name="AILERONS" group ="REAL" failsafe_value="0"/> <!-- IDX 7-->
<axis name="FLAPS" group ="REAL" failsafe_value="0"/> <!-- IDX 8-->
<!-- Virtual actuators commands -->
<axis name="ROLL" group ="VIRTUAL" failsafe_value="0"/> <!-- ID X 9-->
<axis name="PITCH" group ="VIRTUAL" failsafe_value="0"/> <!-- IDX 10-->
<!-- Passive actuators commands -->
<axis name="ROT_MECH" group ="PASSIVE" failsafe_value="0"/> <!-- IDX 11-->
<!-- Legacy commands-->
<axis name="YAW" group ="OTHER" failsafe_value="0"/>
<axis name="THRUST" group ="OTHER" failsafe_value="0"/>
</commands>
<command_laws>
<let VAR="th_hold" VALUE="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
<let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])" SERVO="MOTOR_FRONT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])" SERVO="MOTOR_RIGHT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )" SERVO="MOTOR_BACK"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )" SERVO="MOTOR_LEFT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_PUSHER])" SERVO="MOTOR_PUSH"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : commands[COMMAND_ELEVATOR]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : commands[COMMAND_RUDDER])" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_RIGHT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])" SERVO="FLAP_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])" SERVO="FLAP_RIGHT"/>
<set VALUE="commands[COMMAND_ROT_MECH]" SERVO="ROTATION_MECH"/>
<!-- Backup commands -->
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_FRONT])" SERVO="BMOTOR_FRONT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_RIGHT])" SERVO="BMOTOR_RIGHT"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_BACK] )" SERVO="BMOTOR_BACK"/>
<set VALUE="($th_hold? -9600 : commands[COMMAND_MOTOR_LEFT] )" SERVO="BMOTOR_LEFT"/>
</command_laws>
<section NAME="MISC">
<!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
<define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>
<!-- Others -->
<define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
<define name="NAV_CLIMB_VSPEED" value="2.0" />
<define name="NAV_DESCEND_VSPEED" value="-1.0"/>
<define name="NAV_CARROT_DIST" value="15"/>
<define name="NAV_LINE_DIST" value="100"/>
<define name="CLOSE_TO_WAYPOINT" value="15"/>
<define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="300"/>
<define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
<define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
<define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
<define name="USE_AIRSPEED" value="TRUE"/>
<define name="STABILIZATION_ATTITUDE_SP_MAX_PHI" value="45." unit="deg" />
<define name="STABILIZATION_ATTITUDE_SP_MAX_THETA" value="45." unit="deg" />
<define name="STABILIZATION_ATTITUDE_SP_MAX_R" value="90." unit="deg/s"/>
<define name="STABILIZATION_ATTITUDE_DEADBAND_R" value="200" />
<define name="FWD_SIDESLIP_GAIN" value="0.2"/> <!-- cyfoam 0.32-->
</section>
<section name="GROUND_DETECT">
<define name="THRESHOLD_GROUND_DETECT" value="40"/>
<define name="USE_GROUND_DETECT_INDI_THRUST" value="FALSE"/>
<define name="USE_GROUND_DETECT_AGL_DIST" value="TRUE"/>
<define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE" value="0.24"/>
<define name="AGL_DIST_MIN_DISTANCE_CHECK" value="0.20"/>
<define name="AGL_DIST_MAX_DISTANCE_CHECK" value="0.25"/>
<define name="AGL_DIST_FILTER" value="0.07"/>
</section>
<section name="IMU" prefix="IMU_">
<!-- For some reason accel calibration does not work in JSBSIM. Cannot solve simply in NPS section. -->
<!-- <define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true},.neutral={-12,-9,20}, .scale={{30726,37910,15728},{3133,3871,1611}}}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-9,-30,26}, .scale={{45288,818,33359},{8935,167,6832}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-35,-5,13}, .scale={{26152,56165,62837},{5357,11479,12884}}}}"/> -->
<define name="MAG_CALIB" value="{{.abi_id=5, .calibrated={.neutral=true, .scale=true},.neutral={785,-2641,40}, .scale={{7727,22807,32483},{13208,38799,54820}}}}"/>
<define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
<define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
<define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
</section>
<section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI">
<define name = "MAX_R" value = "120.0" unit="deg/s"/>
<define name = "FILT_CUTOFF" value = "3.0" />
<define name = "FILT_CUTOFF_ACC" value = "3.0"/>
<define name = "FILT_CUTOFF_VEL" value = "5.0"/>
<define name = "FILT_CUTOFF_POS" value = "10.0"/>
<define name = "ESTIMATION_FILT_CUTOFF" value = "3.2" />
<define name = "FILT_CUTOFF_P" value = "3.0"/>
<define name = "FILT_CUTOFF_Q" value = "3.0"/>
<define name = "FILT_CUTOFF_R" value = "3.0" />
<define name = "FILT_CUTOFF_RDOT" value = "0.5" />
<define name = "FILTER_YAW_RATE" value = "TRUE"/>
<!-- | MF | MR | MB | ML | MP | ELE | RUD | AIL | FLA | PHI | THETA | -->
<define name = "ACT_DYN" value = "{ 15.1f, 15.1f, 15.1f, 15.1f, 24.07f, 50.00f, 50.00f, 50.00f, 50.00f, 0.00f, 0.00f }" />
<define name = "ACT_IS_SERVO" value = "{ 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0 }" />
<define name = "ACT_MAX" value = "{ 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, 9600.0f, M_PI_4, M_PI_4}"/>
<define name = "ACT_MIN" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -9600.0f, -9600.0f, -9600.0f, -M_PI_4, -M_PI_4}"/>
<define name = "ACT_MAX_NORM" value = "{ 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f}"/>
<define name = "ACT_MIN_NORM" value = "{ 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f}"/>
<define name = "WU" value = "{ 0.75f, 0.75f, 0.75f, 0.75f, 1.00f, 6.0f, 6.0f, 6.0f, 6.0f, 1.20f, 1.20f}"/>
<define name = "U_PREF" value = "{ 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 8000.0f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f}"/>
<define name = "WV" value = "{ 4.0f, 4.0f, 4.0f, 8.0f, 8.0f, 1.00f}"/>
</section>
<section name="CTRL_EFF_SHED" prefix="ROT_WING_EFF_SCHED_">
<!-- | MF | MR | MB | ML | MP | ELE | RUD | AIL | FLA | PHI | THETA | -->
<!-- <define name = "dFdu" value = "{-0.50f, -0.50f, -0.50f, -0.50f, 0.55f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "dMzdu" value = "{-0.40f, 0.40f, -0.40f, 0.40f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "dMzdu_G2" value = "{-0.02f, 0.02f, -0.02f, 0.02f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f, 0.00f }"/>
<define name = "l" value = "{ 0.423f, 0.408f, 0.423f, 0.408f, 0.00f, 1.00f, 1.00f, 1.00f, 1.00f, 0.00f, 0.00f }"/> -->
<define name="IXX_BODY" value="0.04780"/>
<define name="IYY_BODY" value="0.7546"/>
<define name="IZZ" value="0.9752"/>
<define name="IXX_WING" value="0.08099"/>
<define name="IYY_WING" value="0.1949"/>
<define name="M" value="6.67"/>
<define name="DM_DPPRZ_HOVER_PITCH" value="0.001537287553553"/>
<define name="DM_DPPRZ_HOVER_ROLL" value="{5.86747416762564,0.001936126151564}"/>
<define name="HOVER_ROLL_PITCH_COEF" value="{0.001418491625685,-0.000649367806611}"/>
<define name="HOVER_ROLL_ROLL_COEF" value="{-0.0000839259143513,-0.0000478083}"/>
<define name="K_ELEVATOR" value="{0.4603,-4.81466,-28.8464}"/>
<define name="K_RUDDER" value="{-26.1434,-0.336403,-5.9}"/>
<define name="K_AILERON" value="2.777647188"/>
<define name="K_FLAPERON" value="2.0439"/>
<define name="K_PUSHER" value="{0.007777,-0.67521}"/>
<define name="K_ELEVATOR_DEFLECTION" value="{50.0,-0.0063}"/>
<define name="D_RUDDER_D_PPRZ" value="-0.0018"/>
<define name="K_RPM_PPRZ_PUSHER" value="{-116.518697071689,1.17051409813432,-0.00002580110593734}"/>
<define name="K_LIFT_WING" value="{-0.335811392,-0.615611143}"/>
<define name="K_LIFT_FUSELAGE" value="-0.050653927"/>
<define name="K_LIFT_TAIL" value="-0.101691751"/>
</section>
<section name="AUTOPILOT">
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="MODE_AUTO1" value="AP_MODE_MODULE"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV"/>
<define name="MODE_STARTUP" value="AP_MODE_ATTITUDE_DIRECT"/>
<define name="USE_KILL_SWITCH_FOR_MOTOR_ARMING" value="TRUE"/>
</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="TAKEOFF_BAT_LEVEL" value="24.2" unit="V"/>
<define name="BAT_NB_CELLS" value="6"/>
</section>
<section name="SIMULATOR" prefix="NPS_">
<define name="JSBSIM_MODEL" value="rotwingv3c_SI" type="string"/>
<define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
<define name="USE_COMMANDS" value="TRUE"/>
<define name="JS_AXIS_MODE" value="4"/>
</section>
</airframe>
conf/autopilot/rotorcraft_oneloop_andi_indi.xml
+152 -78
View File
@@ -32,24 +32,22 @@
</settings> </settings>
<control_block name="set_commands"> <control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight(), autopilot_get_motors_on())"/> <call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
</control_block> </control_block>
<control_block name="attitude_loop"> <control_block name="run_attitude_control">
<call fun="stabilization_attitude_read_rc(autopilot_in_flight(), FALSE, FALSE)"/> <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_attitude_run(autopilot_in_flight())"/> <call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block> </control_block>
<control_block name="throttle_direct"> <control_block name="run_guidance_control">
<call fun="guidance_v_read_rc()"/> <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="guidance_v_set_z(stateGetPositionNed_i()->z)"/> <call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/> <call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
</control_block> </control_block>
<control_block name="altitude_loop"> <control_block name="update_v_sp_gcs">
<call fun="gv_update_ref_from_z_sp(guidance_v.z_sp)"/> <call fun="guidance_v_set_z(-nav.nav_altitude)"/>
<call fun="guidance_v.delta_t = guidance_v_run_pos(autopilot_in_flight(), &guidance_v)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = guidance_v.rc_delta_t"/>
</control_block> </control_block>
<exceptions> <exceptions>
@@ -57,6 +55,60 @@
<exception cond="kill_switch_is_on()" deroute="KILL"/> <exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions> </exceptions>
<!-- Kill throttle mode 0-->
<mode name="KILL">
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
<call fun="stabilization.cmd[COMMAND_THRUST] = 0"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
</mode>
<!-- Failsafe mode 1-->
<mode name="FAILSAFE" shortname="FAIL">
<!-- Failsafe does not work needs to be fixed-->
<on_enter>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
</on_enter>
<control>
<call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="guidance_v_update_ref()"/>
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
</mode>
<!-- Home mode 2-->
<mode name="HOME">
<on_enter>
<call fun="guidance_h_nav_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call_block name="run_guidance_control"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>
<!-- Rate mode 3-->
<mode name="RATE_DIRECT" shortname="RATE">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- Attitude mode 4-->
<mode name="ATTITUDE_DIRECT" shortname="ATT"> <mode name="ATTITUDE_DIRECT" shortname="ATT">
<select cond="RCMode0()"/> <select cond="RCMode0()"/>
<on_enter> <on_enter>
@@ -66,47 +118,35 @@
<call fun="nav_periodic_task()"/> <call fun="nav_periodic_task()"/>
</control> </control>
<control> <control>
<call_block name="attitude_loop"/> <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call_block name="throttle_direct"/> <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/> <call_block name="set_commands"/>
</control> </control>
<exception cond="RCLost()" deroute="FAILSAFE"/> <exception cond="RCLost()" deroute="FAILSAFE"/>
</mode> </mode>
<mode name="NAV"> <!-- Rate climb rc mode 5-->
<select cond="RCMode2() && DLModeNav()" exception="HOME"/> <mode name="RATE_RC_CLIMB" shortname="RRCC">
<on_enter>
<call fun="oneloop_andi_enter(false, CTRL_INDI)"/>
</on_enter>
<control>
<call fun="nav_periodic_task()"/>
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/> <exception cond="RCLost()" deroute="FAILSAFE"/>
</mode> </mode>
<mode name="GUIDED"> <!-- ATTITUDE_RC_CLIMB: 6 -->
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/> <mode name="ATTITUDE_RC_CLIMB" shortname="ATTITUDE_RC_CLIMB">
<on_enter> <exception cond="RCLost()" deroute="FAILSAFE"/>
<call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_guided_run(autopilot_in_flight())"/>
<call fun="guidance_h_guided_run(autopilot_in_flight())"/>
<call_block name="set_commands"/>
</control>
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode> </mode>
<!-- ATTITUDE_CLIMB: 7 -->
<mode name="ATTITUDE_CLIMB" shortname="ATTITUDE_CLIMB">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- RATE_Z_HOLD: 8 -->
<mode name="RATE_Z_HOLD" shortname="RATE_Z_HOLD">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- ATTITUDE_Z_HOLD: 9 -->
<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH"> <mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
<select cond="RCMode1() && DLModeAZH()"/> <select cond="RCMode1() && DLModeAZH()"/>
<on_enter> <on_enter>
@@ -128,6 +168,56 @@
<exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/> <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode> </mode>
<!-- HOVER_DIRECT: 10 -->
<mode name="HOVER_DIRECT" shortname="HOVER_DIRECT">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- HOVER_CLIMB: 11 -->
<mode name="HOVER_CLIMB" shortname="HOVER_CLIMB">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- HOVER_Z_HOLD: 12 -->
<mode name="HOVER_Z_HOLD" shortname="HOVER_Z_HOLD">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- NAV: 13 -->
<mode name="NAV">
<select cond="RCMode2() && DLModeNav()" exception="HOME"/>
<on_enter>
<call fun="oneloop_andi_enter(false, CTRL_INDI)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
<call_block name="update_v_sp_gcs"/>
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- RC_DIRECT: 14 -->
<mode name="RC_DIRECT" shortname="RC_DIRECT">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- CARE_FREE_DIRECT: 15 -->
<mode name="CARE_FREE_DIRECT" shortname="CARE_FREE_DIRECT">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- FORWARD: 16 -->
<mode name="FORWARD" shortname="FORWARD">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- MODULE: 17 -->
<mode name="MODULE" shortname="ONE"> <mode name="MODULE" shortname="ONE">
<select cond="RCMode1() && DLModeModule()" exception="HOME"/> <select cond="RCMode1() && DLModeModule()" exception="HOME"/>
<on_enter> <on_enter>
@@ -135,57 +225,41 @@
</on_enter> </on_enter>
<control> <control>
<call fun="nav_periodic_task()"/> <call fun="nav_periodic_task()"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE)"/>
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/> <call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
<call_block name="update_v_sp_gcs"/>
<call fun="oneloop_from_nav(autopilot_in_flight())"/> <call fun="oneloop_from_nav(autopilot_in_flight())"/>
</control> </control>
<exception cond="RCLost()" deroute="FAILSAFE"/> <exception cond="RCLost()" deroute="FAILSAFE"/>
</mode> </mode>
<mode name="HOME"> <!-- FLIP: 18 -->
<mode name="FLIP" shortname="FLIP">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- GUIDED: 19 -->
<mode name="GUIDED">
<select cond="RCMode2() && DLModeGuided()" exception="HOME"/>
<on_enter> <on_enter>
<call fun="guidance_h_nav_enter()"/> <call fun="guidance_h_hover_enter()"/>
<call fun="stabilization_attitude_enter()"/>
<call fun="guidance_v.mode = GUIDANCE_V_MODE_GUIDED"/>
<call fun="guidance_v_guided_enter()"/>
</on_enter> </on_enter>
<control freq="NAVIGATION_FREQUENCY"> <control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/> <call fun="nav_periodic_task()"/>
</control> </control>
<control> <control>
<call fun="guidance_v_read_rc()"/> <call fun="guidance_v_read_rc()"/>
<call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/> <call fun="guidance_v_thrust_adapt(autopilot_in_flight())"/>
<call fun="guidance_v_from_nav(autopilot_in_flight())"/> <call fun="guidance_v_guided_run(autopilot_in_flight())"/>
<call fun="guidance_h_from_nav(autopilot_in_flight())"/> <call fun="guidance_h_guided_run(autopilot_in_flight())"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>
<!-- Safe landing -->
<mode name="FAILSAFE" shortname="FAIL">
<!-- Failsafe does not work needs to be fixed-->
<on_enter>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
</on_enter>
<control>
<call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="guidance_v_update_ref()"/>
<call fun="guidance_v.delta_t = guidance_v_run_speed(autopilot_in_flight(), &guidance_v)"/>
<call_block name="set_commands"/> <call_block name="set_commands"/>
</control> </control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/> <exception cond="GpsIsLost() && autopilot_in_flight()" deroute="FAILSAFE"/>
</mode> </mode>
<!-- Kill throttle mode -->
<mode name="KILL">
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
<call fun="stabilization_cmd[COMMAND_THRUST] = 0"/>
</on_enter>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
</mode>
</state_machine> </state_machine>
</autopilot> </autopilot>
conf/autopilot/rotorcraft_oneloop_switch.xml
+258
View File
@@ -0,0 +1,258 @@
<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
<autopilot name="Oneloop Autopilot Rotorcraft">
<state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
<includes>
<include name="generated/airframe.h"/>
<include name="autopilot.h"/>
<include name="autopilot_rc_helpers.h"/>
<include name="navigation.h"/>
<include name="guidance.h"/>
<include name="oneloop/oneloop_andi.h"/>
<include name="stabilization/stabilization_attitude.h"/>
<include name="modules/radio_control/radio_control.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
<include name="modules/rot_wing_drone/rotwing_state_V2.h"/>
<define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT" cond="ifndef MODE_MANUAL"/>
<define name="MODE_ONELOOP" value="AP_MODE_MODULE" cond="ifndef MODE_AUTO1"/>
<define name="MODE_AUTO2" value="AP_MODE_NAV" cond="ifndef MODE_AUTO2"/>
<define name="RCLost()" value="(radio_control.status == RC_REALLY_LOST)"/>
<define name="DLModeNav()" value="(autopilot_mode_auto2 == AP_MODE_NAV)"/>
<define name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
<define name="DLModeModule()" value="(MODE_AUTO1 == AP_MODE_MODULE)"/>
<define name="DLModeAZH()" value="(MODE_AUTO1 == AP_MODE_ATTITUDE_Z_HOLD)"/>
<define name="ONELOOP_CONTROLLER" value="TRUE"/>
</includes>
<settings>
<dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
<dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
</settings>
<control_block name="set_commands">
<call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
</control_block>
<control_block name="run_attitude_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="run_guidance_control">
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
<call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
</control_block>
<control_block name="update_v_sp_gcs">
<call fun="guidance_v_set_z(-nav.nav_altitude)"/>
</control_block>
<exceptions>
<exception cond="nav.too_far_from_home" deroute="HOME"/>
<exception cond="kill_switch_is_on()" deroute="KILL"/>
</exceptions>
<!-- Kill throttle mode 0-->
<mode name="KILL">
<select cond="kill_switch_is_on()"/>
<on_enter>
<call fun="autopilot_set_in_flight(false)"/>
<call fun="autopilot_set_motors_on(false)"/>
<call fun="stabilization.cmd[COMMAND_THRUST] = 0"/>
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<call fun="SetCommands(commands_failsafe)"/>
</control>
</mode>
<!-- Failsafe mode 1-->
<mode name="FAILSAFE" shortname="FAIL">
<!-- Failsafe does not work needs to be fixed-->
<on_enter>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
<call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
</on_enter>
<control>
<call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
<call fun="guidance_v_update_ref()"/>
<call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
<call_block name="set_commands"/>
</control>
<exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
</mode>
<!-- Home mode 2-->
<mode name="HOME">
<on_enter>
<call fun="guidance_h_nav_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_home()"/>
</control>
<control>
<call_block name="run_guidance_control"/>
</control>
<exception cond="GpsIsLost()" deroute="FAILSAFE"/>
</mode>
<!-- Rate mode 3-->
<mode name="RATE_DIRECT" shortname="RATE">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- Attitude mode 4-->
<mode name="ATTITUDE_DIRECT" shortname="ATT_Quad">
<select cond="RCMode0()"/>
<on_enter>
<call fun="stabilization_attitude_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<!-- <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE)"/> -->
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,STABILIZATION_ATT_SUBMODE_HEADING)"/>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- Rate climb rc mode 5-->
<mode name="RATE_RC_CLIMB" shortname="RRCC">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- ATTITUDE_RC_CLIMB: 6 -->
<mode name="ATTITUDE_RC_CLIMB" shortname="ATTITUDE_RC_CLIMB">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- ATTITUDE_CLIMB: 7 -->
<mode name="ATTITUDE_CLIMB" shortname="ATTITUDE_CLIMB">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- RATE_Z_HOLD: 8 -->
<mode name="RATE_Z_HOLD" shortname="RATE_Z_HOLD">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- ATTITUDE_Z_HOLD: 9 -->
<mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- HOVER_DIRECT: 10 -->
<mode name="HOVER_DIRECT" shortname="HOVER_DIRECT">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- HOVER_CLIMB: 11 -->
<mode name="HOVER_CLIMB" shortname="HOVER_CLIMB">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- HOVER_Z_HOLD: 12 -->
<mode name="HOVER_Z_HOLD" shortname="HOVER_Z_HOLD">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- NAV: 13 -->
<mode name="NAV">
<select cond="RCMode2() && RCAP2() && DLModeNav()" exception="HOME"/>
<on_enter>
<call fun="oneloop_andi_enter(false, CTRL_INDI)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<!-- <call fun="nav_periodic_task()"/> -->
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
<call_block name="update_v_sp_gcs"/>
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- RC_DIRECT: 14 -->
<mode name="RC_DIRECT" shortname="RC_DIRECT">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- CARE_FREE_DIRECT: 15 -->
<mode name="CARE_FREE_DIRECT" shortname="CARE_FREE_DIRECT">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- FORWARD: 16 -->
<mode name="FORWARD" shortname="FORWARD">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- MODULE: 17 -->
<mode name="MODULE" shortname="ONE">
<select cond="RCMode2() && RCAP0() && DLModeModule()" exception="HOME"/>
<on_enter>
<call fun="oneloop_andi_enter(false, CTRL_ANDI)"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<!-- <call fun="nav_periodic_task()"/> -->
<!-- <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE)"/> -->
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
<call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
<call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
<call_block name="update_v_sp_gcs"/>
<call fun="oneloop_from_nav(autopilot_in_flight())"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- FLIP: 18 -->
<mode name="FLIP" shortname="FLIP">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- GUIDED: 19 -->
<mode name="GUIDED">
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
<!-- Attitude mode fixed wing20-->
<mode name="ATTITUDE_DIRECT_FWD" shortname="ATT_Fwd">
<select cond="RCMode1()"/>
<on_enter>
<call fun="stabilization_attitude_enter()"/>
</on_enter>
<control freq="NAVIGATION_FREQUENCY">
<call fun="nav_periodic_task()"/>
</control>
<control>
<!-- <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD)"/> -->
<call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
<call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,STABILIZATION_ATT_SUBMODE_FORWARD)"/>
<call_block name="run_attitude_control"/>
<call_block name="set_commands"/>
</control>
<exception cond="RCLost()" deroute="FAILSAFE"/>
</mode>
</state_machine>
</autopilot>
conf/flight_plans/tudelft/oneloop_cyberzoo.xml
+2
View File
@@ -2,10 +2,12 @@
<flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Safe TU Delft Cyberzoo" security_height="0.4"> <flight_plan alt="1.0" ground_alt="0" lat0="51.990634" lon0="4.376789" max_dist_from_home="60" name="Safe TU Delft Cyberzoo" security_height="0.4">
<header> <header>
#include "autopilot.h"
#include "modules/datalink/datalink.h" #include "modules/datalink/datalink.h"
#include "modules/energy/electrical.h" #include "modules/energy/electrical.h"
#include "modules/radio_control/radio_control.h" #include "modules/radio_control/radio_control.h"
#include "modules/ahrs/ahrs.h" #include "modules/ahrs/ahrs.h"
#include "modules/gps/gps.h"
<!-- #include "sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h" --> <!-- #include "sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h" -->
</header> </header>
<waypoints> <waypoints>
conf/flight_plans/tudelft/oneloop_valkenburg.xml
+140 -64
View File
@@ -1,38 +1,45 @@
<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd"> <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<flight_plan alt="70.0" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="1070" name="Oneloop Valkenburg" security_height="2"> <flight_plan alt="70.0" ground_alt="0" lat0="52.1681551" lon0="4.4126468" max_dist_from_home="1070" name="Oneloop Valkenburg" security_height="2">
<header> <header>
#include "autopilot.h" #include "autopilot.h"
#include "modules/datalink/datalink.h" #include "modules/datalink/datalink.h"
#include "modules/energy/electrical.h" #include "modules/energy/electrical.h"
#include "modules/radio_control/radio_control.h" #include "modules/radio_control/radio_control.h"
#include "modules/ahrs/ahrs.h" #include "modules/ahrs/ahrs.h"
#include "modules/gps/gps.h"
</header> </header>
<waypoints> <waypoints>
<waypoint name="HOME" x="12.6" y="-48.7"/> <waypoint name="HOME" lat="52.1681551" lon="4.4126468" alt="70.0" />
<waypoint name="CLIMB" x="62.6" y="-80.4"/> <waypoint name="CLIMB" lat="52.1687915" lon="4.4153418" alt="70.0" />
<waypoint name="STDBY" lat="52.1682655" lon="4.4135103"/> <waypoint name="trans" lat="52.1687890" lon="4.4153418" alt="70.0" />
<waypoint name="p1" x="322.0" y="-254.3"/> <waypoint name="decel" lat="52.1687875" lon="4.4153418" alt="70.0" />
<waypoint name="p2" x="696.0" y="-148.1"/> <waypoint name="begin_trans" lat="52.1687905" lon="4.4153418" alt="70.0" />
<waypoint name="p3" x="600.4" y="73.4"/> <waypoint name="end_trans" lat="52.1687880" lon="4.4153418" alt="70.0" />
<waypoint name="p4" x="238.6" y="-48.5"/> <waypoint name="STDBY" lat="52.1682138" lon="4.4136350" alt="70.0" />
<waypoint name="TD" lat="52.1681602" lon="4.4127708"/> <waypoint name="p1" lat="52.1661207" lon="4.4153418" alt="70.0" />
<waypoint name="FOLLOW" lat="52.16850964562752" lon="4.413635008734417"/> <waypoint name="p2" lat="52.1674823" lon="4.4180372" alt="70.0" />
<waypoint name="S1" lat="52.1669450" lon="4.4174372"/> <waypoint name="p3" lat="52.1687917" lon="4.4173814" alt="70.0" />
<waypoint name="S2" lat="52.1689871" lon="4.4208804"/> <waypoint name="p4" lat="52.1679564" lon="4.4149828" alt="70.0" />
<waypoint lat="52.169189" lon="4.410820" name="C1"/> <waypoint name="circ" lat="52.1674456" lon="4.4161317" alt="70.0" />
<waypoint lat="52.168049" lon="4.406923" name="C2"/> <waypoint name="TD" lat="52.1681531" lon="4.4126768" alt="70.0" />
<waypoint lat="52.166515" lon="4.408235" name="C3"/> <waypoint name="Descend" lat="52.1681246" lon="4.4128048" alt="20.0" />
<waypoint lat="52.163255" lon="4.407668" name="C4"/> <waypoint name="FOLLOW" lat="52.1685096" lon="4.4136350" alt="70.0" />
<waypoint lat="52.161908" lon="4.410082" name="C5"/> <waypoint name="S1" lat="52.1669450" lon="4.4174372" alt="70.0" />
<waypoint lat="52.162641" lon="4.416992" name="C6"/> <waypoint name="S2" lat="52.1689871" lon="4.4208804" alt="70.0" />
<waypoint lat="52.164861" lon="4.427268" name="C7"/> <waypoint name="C1" lat="52.169189" lon="4.410820" alt="70.0" />
<waypoint lat="52.170422" lon="4.427511" name="C8"/> <waypoint name="C2" lat="52.168049" lon="4.406923" alt="70.0" />
<waypoint lat="52.172276" lon="4.424011" name="C9"/> <waypoint name="C3" lat="52.166515" lon="4.408235" alt="70.0" />
<waypoint lat="52.1673931" lon="4.4127541" name="p5"/> <waypoint name="C4" lat="52.163255" lon="4.407668" alt="70.0" />
<waypoint lat="52.1678569" lon="4.4150577" name="p6"/> <waypoint name="C5" lat="52.161908" lon="4.410082" alt="70.0" />
<waypoint lat="52.1689993" lon="4.4144979" name="p7"/> <waypoint name="C6" lat="52.162641" lon="4.416992" alt="70.0" />
<waypoint lat="52.1686045" lon="4.4121999" name="p8"/> <waypoint name="C7" lat="52.164861" lon="4.427268" alt="70.0" />
<waypoint name="C8" lat="52.170422" lon="4.427511" alt="70.0" />
<waypoint name="C9" lat="52.172276" lon="4.424011" alt="70.0" />
<waypoint name="p5" lat="52.1626367" lon="4.4124884" alt="70.0" />
<waypoint name="p6" lat="52.1650926" lon="4.4257087" alt="70.0" />
<waypoint name="p7" lat="52.1709078" lon="4.4220382" alt="70.0" />
<waypoint name="p8" lat="52.1685456" lon="4.4103268" alt="70.0" />
</waypoints> </waypoints>
<sectors> <sectors>
<sector color="red" name="Hard_Geofence"> <sector color="red" name="Hard_Geofence">
@@ -54,18 +61,17 @@
</sector> </sector>
</sectors> </sectors>
<modules> <modules>
<!--module name="follow_me"/--> <!--<module name="follow_me">
<module name="nav" type="survey_rectangle_rotorcraft"> <define name="FOLLOW_ME_MOVING_WPS" value="WP_p1,WP_p2,WP_p3,WP_p4,WP_STDBY,WP_circ,WP_APP"/>
<define name="RECTANGLE_SURVEY_DEFAULT_SWEEP" value="100"/> </module> -->
</module>
</modules> </modules>
<exceptions> <exceptions>
<!--Soft Geofencing (go back to Standby)--> <!--Soft Geofencing (go back to Standby)-->
<exception cond="Or(!InsideSoft_Geofence(GetPosX(), GetPosY()), GetPosAlt() @GT 80.0) @AND <exception cond="Or(!InsideSoft_Geofence(GetPosX(), GetPosY()), GetPosAlt() @GT 100.0) @AND
!(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND
!(nav_block == IndexOfBlock('Geo init'))" deroute="safe"/> !(nav_block == IndexOfBlock('Geo init'))" deroute="safe"/>
<!-- Hard Geofencing (Kill) --> <!-- Hard Geofencing (Kill) -->
<exception cond="(Or(!InsideHard_Geofence(GetPosX(), GetPosY()), GetPosAlt() @GT 80.0) @AND <exception cond="(Or(!InsideHard_Geofence(GetPosX(), GetPosY()), GetPosAlt() @GT 100.0) @AND
!(IndexOfBlock('Holding point') @GT nav_block) @AND !(IndexOfBlock('Holding point') @GT nav_block) @AND
!(nav_block >= IndexOfBlock('land here')) @AND !(nav_block >= IndexOfBlock('land here')) @AND
(autopilot_in_flight() == true) )" deroute="Landed"/> (autopilot_in_flight() == true) )" deroute="Landed"/>
@@ -92,69 +98,139 @@
</exceptions> </exceptions>
<blocks> <blocks>
<block name="Wait GPS"> <block name="Wait GPS">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavKillThrottle()"/> <call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/> <while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block> </block>
<block name="Geo init"> <block name="Geo init" strip_button="Geo_init" strip_icon="googleearth.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/> <while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/> <call_once fun="NavSetAltitudeReferenceHere()"/>
</block> </block>
<block name="Holding point"> <block name="Holding point" strip_button="Holding point" strip_icon="off.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavKillThrottle()"/> <call_once fun="NavKillThrottle()"/>
<set var="commands[COMMAND_MOTOR_PUSHER]" value="0"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block> </block>
<block key="r" name="Start Engine"> <block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<call_once fun="NavResurrect()"/> <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
<call_once fun="NavResurrect()"/> <call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
</block> </block>
<block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png"> <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<exception cond="GetPosAlt() @GT 0.8" deroute="Standby"/> <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/> <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
</block>
<block name="Climb" strip_button="Climb" strip_icon="up.png">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/> <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block> </block>
<block name="Pre-Standby"> <block name="Standby" strip_button="Standby" strip_icon="wp_quad.png">
<while cond="oneloop_andi.half_loop"/> <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<!-- <call_once fun="NavSetWaypointPosAndAltHere(WP_STDBY)"/> --> <call_once fun="rotwing_state_force_skew_off()"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block> </block>
<block key="s" name="Standby" strip_button="Standby" strip_icon="home.png"> <block name="Standby_fwd" strip_button="Standby_fwd" strip_icon="wp_fwd.png">
<stay wp="STDBY"/> <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<call_once fun="rotwing_state_force_skew_off()"/>
<stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block> </block>
<block name="safe"> <block name="Descend" strip_button="Descend" strip_icon="descend.png">
<stay wp="HOME"/> <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_force_skew_off()"/>
<stay wp="Descend" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block> </block>
<block name="Circle_CW"> <!-- <block name="stay_begin-trans">
<set value="false" var="force_forward"/> <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<circle radius="100" wp="FOLLOW"/> <stay wp="begin_trans" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block> </block>
<block key="l" name="land here" strip_button="land here" strip_icon="land-right.png"> <block name="end_transition">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay wp="end_trans"/>
</block>-->
<block name="transition_CW" strip_button="transition_CW" strip_icon="transition_right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<stay wp="end_trans"/>
<exception cond="RotWingAutomationReadyForForward()" deroute="Circle_CW_fwd"/>
</block>
<block name="transition_CCW" strip_button="transition_CCW" strip_icon="transition_left.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<stay wp="end_trans"/>
<exception cond="RotWingAutomationReadyForForward()" deroute="Circle_CCW_fwd"/>
</block>
<block name="safe" strip_button="Unsafe" strip_icon="alert.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay wp="HOME" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Circle_CW_fwd" strip_button="CircleCW_Forward" strip_icon="circle_cw_fwd.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<call_once fun="rotwing_state_force_skew_off()"/>
<circle radius="100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Circle_CCW_fwd" strip_button="CircleCCW_Forward" strip_icon="circle_ccw_fwd.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<call_once fun="rotwing_state_force_skew_off()"/>
<circle radius="-100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Circle_CW_quad" strip_button="CircleCW_Quad" strip_icon="circle_cw_quad.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_force_skew_off()"/>
<circle radius="100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="Circle_CCW_quad" strip_button="CircleCCW_Quad" strip_icon="circle_ccw_quad.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_force_skew_off()"/>
<circle radius="-100" wp="circ" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
</block>
<block name="route_fw" strip_button="Route_fwd" strip_icon="path.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<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_fw"/>
</block>
<block name="land here" strip_button="Land_Here" strip_icon="land-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/> <call_once fun="NavSetWaypointHere(WP_TD)"/>
<go wp="TD"/>
<deroute block="Flare"/>
</block> </block>
<block name="Land"> <block name="land" strip_button="Land" strip_icon="land-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<go wp="TD"/> <go wp="TD"/>
<deroute block="Flare"/>
</block> </block>
<block name="Flare"> <block name="descend" strip_button="Descend" strip_icon="down.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="GetPosHeight() @LT 12.0" deroute="flare"/>
<stay climb="-1.0" vmode="climb" wp="TD"/>
</block>
<block name="flare" strip_button="Flare" strip_icon="downdown.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<stay climb="-0.5" vmode="climb" wp="TD"/>
<!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
<exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
</block>
<block name="flare_low" strip_button="FlareLow" strip_icon="downdown.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<exception cond="NavDetectGround()" deroute="Holding point"/> <exception cond="NavDetectGround()" deroute="Holding point"/>
<exception cond="!nav_is_in_flight()" deroute="Landed"/> <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="0.10 @GT GetPosAlt()" deroute="Landed"/> <exception cond="ground_detect()" deroute="Holding point"/>
<call_once fun="NavStartDetectGround()"/> <call_once fun="NavStartDetectGround()"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/> <stay climb="-0.5" vmode="climb" wp="TD"/>
</block> </block>
<block name="Safe landing"> <block name="Manual" strip_button="Manual" strip_icon="joystick.png">
<exception cond="!nav_is_in_flight()" deroute="Manual"/>
<exception cond="0.10 @GT GetPosAlt()" deroute="Manual"/>
<call_once fun="NavStartDetectGround()"/>
<stay climb="nav.descend_vspeed" vmode="climb" wp="TD"/>
</block>
<block name="Manual">
<set value="AP_MODE_ATTITUDE_DIRECT" var="autopilot_mode_auto2"/> <set value="AP_MODE_ATTITUDE_DIRECT" var="autopilot_mode_auto2"/>
<stay wp="STDBY"/> <stay wp="STDBY"/>
</block> </block>
<block name="Landed"> <block name="Landed">
<call_once fun="NavKillThrottle()"/> <call_once fun="NavKillThrottle()"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/> <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block> </block>
</blocks> </blocks>
conf/modules/oneloop_andi.xml
+5 -1
View File
@@ -6,6 +6,10 @@
<settings> <settings>
<dl_settings> <dl_settings>
<dl_settings NAME="oneloop"> <dl_settings NAME="oneloop">
<dl_setting var="max_as" min="7" step="1" max="19" shortname="max_airspeed"/>
<dl_setting var="temp_pitch" min="0" step="1" max="9600" shortname="temp_pitch"/>
<dl_setting var="k_as" min="1.0" step="0.001" max="10" shortname="k_as"/>
<dl_setting var="ctrl_off" min="0" step="1" max="1" values="OFF|ON" shortname="ctrl_off"/>
<dl_setting var="chirp_on" min="0" step="1" max="1" values="OFF|ON" shortname="chirp_on"/> <dl_setting var="chirp_on" min="0" step="1" max="1" values="OFF|ON" shortname="chirp_on"/>
<dl_setting var="chirp_axis" min="0" step="1" max="3" shortname="chirp_axis"/> <dl_setting var="chirp_axis" min="0" step="1" max="3" shortname="chirp_axis"/>
<dl_setting var="f0_chirp" min="0.01" step="0.001" max="10" shortname="f0_chirp"/> <dl_setting var="f0_chirp" min="0.01" step="0.001" max="10" shortname="f0_chirp"/>
@@ -14,7 +18,7 @@
<dl_setting var="A_chirp" min="0.01" step="0.001" max="10" shortname="A_chirp"/> <dl_setting var="A_chirp" min="0.01" step="0.001" max="10" shortname="A_chirp"/>
<dl_setting var="heading_manual" min="0" step="1" max="1" values="OFF|ON" shortname="take_heading"/> <dl_setting var="heading_manual" min="0" step="1" max="1" values="OFF|ON" shortname="take_heading"/>
<dl_setting var="yaw_stick_in_auto" min="0" step="1" max="1" values="OFF|ON" shortname="yaw_stick_on"/> <dl_setting var="yaw_stick_in_auto" min="0" step="1" max="1" values="OFF|ON" shortname="yaw_stick_on"/>
<dl_setting var="fwd_sideslip_gain" min="0.1" step="0.001" max="20.0" shortname="fwd_sideslip_gain"/> <dl_setting var="fwd_sideslip_gain" min="0.01" step="0.001" max="20.0" shortname="fwd_sideslip_gain"/>
<dl_setting var="psi_des_deg" min="-180.0" step="0.1" max="180.0" shortname="psi_des"/> <dl_setting var="psi_des_deg" min="-180.0" step="0.1" max="180.0" shortname="psi_des"/>
<dl_setting var="p_att_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_att_e_omega_n"/> <dl_setting var="p_att_e.omega_n" min="0.1" step="0.001" max="20.0" shortname="p_att_e_omega_n"/>
<dl_setting var="p_att_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_att_e_zeta"/> <dl_setting var="p_att_e.zeta" min="0.1" step="0.001" max="1.0" shortname="p_att_e_zeta"/>
conf/simulator/jsbsim/aircraft/rotwingv3c_SI.xml
+685 -129
View File
File diff suppressed because it is too large Load Diff
conf/userconf/tudelft/RW_control_panel.xml
+1503
View File
File diff suppressed because it is too large Load Diff
conf/userconf/tudelft/conf.xml
+51 -7
View File
@@ -582,6 +582,50 @@
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.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_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml" settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.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_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
gui_color="red" gui_color="red"
/> />
<aircraft
name="Bebop_DePonti"
ac_id="36"
airframe="airframes/tudelft/bebop_one_indi_generated_ekf2.xml"
radio="radios/dummy.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/oneloop_cyberzoo.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/ins_ekf2.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/stabilization_indi.xml"
gui_color="#ffffcccaccca"
/>
<aircraft
name="Bebop2_DePonti"
ac_id="37"
airframe="airframes/tudelft/bebop2_oneloop.xml"
radio="radios/dummy.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/oneloop_valkenburg.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.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_simple.xml] modules/stabilization_indi.xml modules/oneloop_andi.xml"
gui_color="#ffffcccaccca"
/>
<aircraft
name="RW3C_DePonti"
ac_id="35"
airframe="airframes/tudelft/rot_wing_v3c_oneloop.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/oneloop_valkenburg.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rot_wing_V2.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rot_wing_automation.xml modules/rotwing_state_V2.xml"
gui_color="#ffffcccaccca"
/>
<aircraft
name="RW3C_DePonti_Simulation"
ac_id="34"
airframe="airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml"
radio="radios/dummy.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/oneloop_valkenburg.xml"
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rot_wing_V2.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rot_wing_automation.xml modules/rotwing_state_V2.xml"
gui_color="#ffffcccaccca"
/>
<aircraft <aircraft
name="RotatingWingV3F" name="RotatingWingV3F"
ac_id="10" ac_id="10"
@@ -616,14 +660,14 @@
gui_color="red" gui_color="red"
/> />
<aircraft <aircraft
name="RotatingWingV3C" name="RW3_AG"
ac_id="34" ac_id="38"
airframe="airframes/tudelft/rot_wing_v3c_oneloop.xml" airframe="airframes/AG_UAV/rot_wing_v3c_oneloop_optitrack_ekf2_AG.xml"
radio="radios/crossfire_sbus.xml" radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml" telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/oneloop_valkenburg.xml" flight_plan="flight_plans/tudelft/oneloop_cyberzoo.xml"
settings="settings/rotorcraft_basic.xml" settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/oneloop_andi.xml" settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/eff_scheduling_rot_wing.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml [modules/guidance_rotorcraft.xml] modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/oneloop_andi.xml modules/rot_wing_automation.xml modules/rotwing_state.xml"
gui_color="red" gui_color="#ffffcccaccca"
/> />
</conf> </conf>
data/pictures/gcs_icons/transition_left.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 46 KiB

data/pictures/gcs_icons/transition_right.png
BIN
View File
Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 83 KiB

sw/airborne/firmwares/rotorcraft/guidance/guidance_oneloop.c
+7 -6
View File
@@ -30,6 +30,7 @@
#include "firmwares/rotorcraft/guidance/guidance_v.h" #include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_oneloop.h" #include "firmwares/rotorcraft/guidance/guidance_oneloop.h"
#include "firmwares/rotorcraft/oneloop/oneloop_andi.h" #include "firmwares/rotorcraft/oneloop/oneloop_andi.h"
struct ThrustSetpoint thrust_sp;
void guidance_h_run_enter(void) void guidance_h_run_enter(void)
{ {
@@ -59,25 +60,25 @@ struct StabilizationSetpoint guidance_h_run_accel(bool in_flight, struct Horizon
return guidance_oneloop_run_mode(in_flight, gh, _gv, GUIDANCE_ONELOOP_H_ACCEL, _v_mode); return guidance_oneloop_run_mode(in_flight, gh, _gv, GUIDANCE_ONELOOP_H_ACCEL, _v_mode);
} }
int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv) struct ThrustSetpoint guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
{ {
_gv = gv; _gv = gv;
_v_mode = GUIDANCE_ONELOOP_V_POS; _v_mode = GUIDANCE_ONELOOP_V_POS;
return 0; // nothing to do return thrust_sp; // nothing to do
} }
int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv) struct ThrustSetpoint guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
{ {
_gv = gv; _gv = gv;
_v_mode = GUIDANCE_ONELOOP_V_SPEED; _v_mode = GUIDANCE_ONELOOP_V_SPEED;
return 0; // nothing to do return thrust_sp; // nothing to do
} }
int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv) struct ThrustSetpoint guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
{ {
_gv = gv; _gv = gv;
_v_mode = GUIDANCE_ONELOOP_V_ACCEL; _v_mode = GUIDANCE_ONELOOP_V_ACCEL;
return 0; // nothing to do return thrust_sp;; // nothing to do
} }
struct StabilizationSetpoint guidance_oneloop_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceOneloop_HMode h_mode, enum GuidanceOneloop_VMode v_mode) struct StabilizationSetpoint guidance_oneloop_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceOneloop_HMode h_mode, enum GuidanceOneloop_VMode v_mode)
sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c
+506 -256
View File
File diff suppressed because it is too large Load Diff
sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h
+6 -2
View File
@@ -57,6 +57,7 @@
#define CTRL_ANDI 0 #define CTRL_ANDI 0
#define CTRL_INDI 1 #define CTRL_INDI 1
extern bool ctrl_off;
extern float act_state_filt_vect_1l[ANDI_NUM_ACT]; extern float act_state_filt_vect_1l[ANDI_NUM_ACT];
extern float actuator_state_1l[ANDI_NUM_ACT]; extern float actuator_state_1l[ANDI_NUM_ACT];
extern float nu[6]; extern float nu[6];
@@ -69,6 +70,8 @@ extern bool yaw_stick_in_auto;
extern float fwd_sideslip_gain; extern float fwd_sideslip_gain;
extern struct FloatEulers eulers_zxy_des; extern struct FloatEulers eulers_zxy_des;
extern float psi_des_rad; extern float psi_des_rad;
extern float k_as;
extern float max_as;
/*Chirp test Variables*/ /*Chirp test Variables*/
extern bool chirp_on; extern bool chirp_on;
@@ -90,7 +93,7 @@ struct guidance_indi_hybrid_params {
float liftd_p50; float liftd_p50;
}; };
extern struct guidance_indi_hybrid_params gih_params; extern struct guidance_indi_hybrid_params gih_params;
extern bool force_forward; //extern bool force_forward;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
struct OneloopGuidanceRef { struct OneloopGuidanceRef {
float pos[3]; float pos[3];
@@ -144,6 +147,7 @@ struct Gains2ndOrder{
float k3; float k3;
}; };
extern int16_t temp_pitch;
/*Declaration of Reference Model and Error Controller Gains*/ /*Declaration of Reference Model and Error Controller Gains*/
extern struct PolePlacement p_att_e; extern struct PolePlacement p_att_e;
extern struct PolePlacement p_att_rm; extern struct PolePlacement p_att_rm;
@@ -167,7 +171,7 @@ extern void oneloop_andi_init(void);
extern void oneloop_andi_enter(bool half_loop_sp, int ctrl_type); extern void oneloop_andi_enter(bool half_loop_sp, int ctrl_type);
extern void oneloop_andi_set_failsafe_setpoint(void); extern void oneloop_andi_set_failsafe_setpoint(void);
extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v); extern void oneloop_andi_run(bool in_flight, bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v);
extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v); extern void oneloop_andi_RM(bool half_loop, struct FloatVect3 PSA_des, int rm_order_h, int rm_order_v, bool in_flight_oneloop);
extern void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn); extern void oneloop_andi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
extern void oneloop_from_nav(bool in_flight); extern void oneloop_from_nav(bool in_flight);
#endif // ONELOOP_ANDI_H #endif // ONELOOP_ANDI_H