Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 13:24:03 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[rotwing] Update state machine (#3397)

Browse Source 
* first push

* starting to fix make modules

* Fixed test modules

* Update flight plan valkenburgh

* fix define
This commit is contained in:
Tomaso Maria Luigi De Ponti
2024-10-30 16:47:21 +01:00
committed by GitHub
parent bdad22ca96
commit 655a496f7e
17 changed files with 601 additions and 1138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/AG/rot_wing_v3c_oneloop_optitrack_ext_pose_AG.xml
+1 -3
View File
@@ -133,9 +133,7 @@
</module>
<module name="ground_detect"/>
<module name="rotwing_state_V2">
<define name="RW_USE_MODULES_V2" value="TRUE"/>
</module>
<module name="rotwing_state"/>
<module name="agl_dist"/>
</firmware>
conf/airframes/tudelft/rotwing_v3c_oneloop.xml
+1 -3
View File
@@ -138,9 +138,7 @@
</module>
<module name="ground_detect"/>
<module name="rotwing_state_V2">
<define name="RW_USE_MODULES_V2" value="TRUE"/>
</module>
<module name="rotwing_state"/>
<module name="agl_dist"/>
</firmware>
conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack.xml
+1 -3
View File
@@ -140,9 +140,7 @@
</module>
<module name="ground_detect"/>
<module name="rotwing_state_V2">
<define name="RW_USE_MODULES_V2" value="TRUE"/>
</module>
<module name="rotwing_state"/>
<module name="agl_dist"/>
</firmware>
conf/airframes/tudelft/rotwing_v3c_oneloop_optitrack_ext_pose.xml
+18 -5
View File
@@ -131,9 +131,7 @@
</module>
<module name="ground_detect"/>
<module name="rotwing_state_V2">
<define name="RW_USE_MODULES_V2" value="TRUE"/>
</module>
<module name="rotwing_state"/>
<module name="agl_dist"/>
</firmware>
@@ -216,7 +214,24 @@
<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="ROTWING" prefix="ROTWING_">
<define name="FW_MIN_AIRSPEED" value="17.0"/> <!-- Forward stall airspeed + margin (motors off) -->
<define name="FW_QUAD_MIN_AIRSPEED" value="15.0"/> <!-- Forward stall airspeed + margin with quad motors on -->
<define name="FW_CRUISE_AIRSPEED" value="19.0"/> <!-- Default cruise airspeed -->
<define name="FW_MAX_AIRSPEED" value="22.0"/> <!-- Maximum forward airspeed -->
<define name="FW_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in fixed wing mode -->
<define name="QUAD_NOPUSH_AIRSPEED" value="5.0"/> <!-- Maximum quadrotor without pusher motor airspeed -->
<define name="QUAD_MAX_AIRSPEED" value="12.0"/> <!-- Maximum quadrotor airspeed (with pusher motor)-->
<define name="QUAD_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in quad mode -->
<define name="SKEW_UP_AIRSPEED" value="10.0"/> <!-- Airspeed where the skewing starts when going up in airspeed -->
<define name="SKEW_DOWN_AIRSPEED" value="8.0"/> <!-- Airspeed where the skewing starts when going down in airspeed -->
<define name="SKEW_REF_MODEL" value="TRUE"/> <!-- Enable second order reference model for the skewing command -->
<define name="SKEW_REF_MODEL_P_GAIN" value="0.001"/> <!-- Skewing reference model proportional gain -->
<define name="SKEW_REF_MODEL_D_GAIN" value="0.003"/> <!-- Skewing reference model differential gain -->
<define name="SKEW_REF_MODEL_MAX_SPEED" value="20"/> <!-- Maximum rotational skewing speed bound for the reference model -->
<define name="FW_SKEW_ANGLE" value="85"/>
</section>
<section NAME="MISC">
<!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
@@ -237,8 +252,6 @@
<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"/>
<define name="INS_EXT_VISION_ROTATION" value="TRUE"/>
</section>
<section name="GROUND_DETECT">
conf/airframes/tudelft/rotwing_v3c_oneloop_simulation.xml
+18 -25
View File
@@ -99,11 +99,7 @@
</module>
<module name="ground_detect"/>
<module name="rotwing_state_V2">
<define name = "USE_ROTMECH_VIRTUAL" value = "TRUE"/>
<define name = "ROTMECH_DYN" value = "3.0"/>
<define name="RW_USE_MODULES_V2" value="TRUE"/>
</module>
<module name="rotwing_state"/>
<module name="agl_dist"/>
</firmware>
@@ -186,7 +182,24 @@
<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="ROTWING" prefix="ROTWING_">
<define name="FW_MIN_AIRSPEED" value="17.0"/> <!-- Forward stall airspeed + margin (motors off) -->
<define name="FW_QUAD_MIN_AIRSPEED" value="15.0"/> <!-- Forward stall airspeed + margin with quad motors on -->
<define name="FW_CRUISE_AIRSPEED" value="19.0"/> <!-- Default cruise airspeed -->
<define name="FW_MAX_AIRSPEED" value="22.0"/> <!-- Maximum forward airspeed -->
<define name="FW_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in fixed wing mode -->
<define name="QUAD_NOPUSH_AIRSPEED" value="5.0"/> <!-- Maximum quadrotor without pusher motor airspeed -->
<define name="QUAD_MAX_AIRSPEED" value="12.0"/> <!-- Maximum quadrotor airspeed (with pusher motor)-->
<define name="QUAD_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in quad mode -->
<define name="SKEW_UP_AIRSPEED" value="10.0"/> <!-- Airspeed where the skewing starts when going up in airspeed -->
<define name="SKEW_DOWN_AIRSPEED" value="8.0"/> <!-- Airspeed where the skewing starts when going down in airspeed -->
<define name="SKEW_REF_MODEL" value="TRUE"/> <!-- Enable second order reference model for the skewing command -->
<define name="SKEW_REF_MODEL_P_GAIN" value="0.001"/> <!-- Skewing reference model proportional gain -->
<define name="SKEW_REF_MODEL_D_GAIN" value="0.003"/> <!-- Skewing reference model differential gain -->
<define name="SKEW_REF_MODEL_MAX_SPEED" value="20"/> <!-- Maximum rotational skewing speed bound for the reference model -->
<define name="FW_SKEW_ANGLE" value="85"/>
</section>
<section NAME="MISC">
<!-- Voltage and current measurements -->
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
@@ -264,26 +277,6 @@
<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">
conf/airframes/tudelft/rotwing_v3g_oneloop_optitrack_ext_pose.xml
+373
View File
@@ -0,0 +1,373 @@
<!-- This is a 7kg Rotating Wing Drone G
* Airframe: TUD00362
* Autopilot: Cube orange+
* Datalink: Herelink
* GPS: UBlox F9P
* RC: SBUS Crossfire
-->
<airframe name="RotatingWingV3G">
<description>RotatingWingV3G with optitrack and INS ext pose</description>
<firmware name="rotorcraft">
<autopilot name="rotorcraft_oneloop_switch.xml"/>
<target name="ap" board="cube_orangeplus">
<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 EXT POSE configure inputs -->
<define name="INS_EXT_POSE_MAG_ID" value="MAG_RM3100_SENDER_ID"/>
<define name="INS_EXT_POSE_IMU_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"/>
<!-- Range sensor connected to supercan -->
<module name="range_sensor_uavcan"/>
<!-- 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="B460800"/> <!-- 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">
<configure name="MS45XX_I2C_DEV" value="i2c2"/>
<define name="MS45XX_PRESSURE_SCALE" value="1.90"/>
<define name="USE_AIRSPEED_LOWPASS_FILTER" value="TRUE"/>
<define name="MS45XX_LOWPASS_TAU" value="0.25"/>
<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="ext_pose"/>
<!-- 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_SOFT_ACCELERATION" value="2.0f"/>
<define name="NAV_HYBRID_MAX_DECELERATION" value="1.0f"/>
<define name="GUIDANCE_H_USE_REF" value="FALSE"/>
<define name="NAV_HYBRID_EXT_VISION_SETPOINT_MODE" value="TRUE"/>
<define name="NAV_HYBRID_LIMIT_CIRCLE_RADIUS" value="TRUE"/>
</module>
<module name="oneloop" type="andi">
<configure name="ANDI_OUTPUTS" value="6"/>
</module>
<module name="eff_scheduling_rotwing_V2"/>
<module name="wls">
<define name="WLS_N_U_MAX" value = "11"/>
<define name="WLS_N_V_MAX" value = "6"/>
</module>
<module name="ground_detect"/>
<module name="rotwing_state"/>
<module name="agl_dist"/>
</firmware>
<!-- Can bus 1 actuators -->
<servos driver="Uavcan1">
<servo no="0" name="MOTOR_FRONT" min="0" neutral="600" max="7372"/>
<servo no="1" name="MOTOR_RIGHT" min="0" neutral="600" max="7372"/>
<servo no="2" name="MOTOR_BACK" min="0" neutral="600" max="7372"/>
<servo no="3" name="MOTOR_LEFT" min="0" neutral="600" max="7372"/>
<servo no="4" name="MOTOR_PUSH" min="0" neutral="200" max="7372"/>
<servo no="5" name="ROTATION_MECH" min="-1671" neutral="188" max="2048"/>
</servos>
<!-- CAN BUS 1 command outputs-->
<servos driver="Uavcan1Cmd">
<servo no="6" name="SERVO_ELEVATOR" min="5400" neutral="5400" max="-4349"/>
<servo no="7" name="SERVO_RUDDER" min="-4750" neutral="0" max="4750"/>
</servos>
<!-- Can bus 2 actuators -->
<servos driver="Uavcan2">
<servo no="0" name="BMOTOR_FRONT" min="0" neutral="600" max="7372"/>
<servo no="1" name="BMOTOR_RIGHT" min="0" neutral="600" max="7372"/>
<servo no="2" name="BMOTOR_BACK" min="0" neutral="600" max="7372"/>
<servo no="3" name="BMOTOR_LEFT" min="0" neutral="600" max="7372"/>
<servo no="5" name="BROTATION_MECH" min="-1671" neutral="188" max="2048"/>
</servos>
<!-- CAN BUS 2 command outputs-->
<servos driver="Uavcan2Cmd">
<servo no="8" name="AIL_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
<servo no="9" name="FLAP_LEFT" min="-3250" neutral="0" max="3250"/> <!-- min can go up to -9600-->
<servo no="10" name="FLAP_RIGHT" min="-3250" neutral="0" max="3250"/> <!-- max can go up to -9600-->
<servo no="11" 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)"/>
<let VAR="hover_off" VALUE="Or($th_hold, !rotwing_state.hover_motors_enabled)"/>
<let var="ail_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 20)"/>
<let var="flp_limit_hit" value="LessThan(rotwing_state.meas_skew_angle_deg, 50)"/>
<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="$ail_limit_hit? 0:($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_LEFT"/>
<set VALUE="$ail_limit_hit? 0:($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_AILERONS])" SERVO="AIL_RIGHT"/>
<set VALUE="$flp_limit_hit? 0:($servo_hold? RadioControlValues(RADIO_ROLL) : commands[COMMAND_FLAPS])" SERVO="FLAP_LEFT"/>
<set VALUE="$flp_limit_hit? 0:($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"/>
<set VALUE="commands[COMMAND_ROT_MECH]" SERVO="BROTATION_MECH"/>
</command_laws>
<section name="ROTWING" prefix="ROTWING_">
<define name="FW_MIN_AIRSPEED" value="17.0"/> <!-- Forward stall airspeed + margin (motors off) -->
<define name="FW_QUAD_MIN_AIRSPEED" value="15.0"/> <!-- Forward stall airspeed + margin with quad motors on -->
<define name="FW_CRUISE_AIRSPEED" value="19.0"/> <!-- Default cruise airspeed -->
<define name="FW_MAX_AIRSPEED" value="22.0"/> <!-- Maximum forward airspeed -->
<define name="FW_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in fixed wing mode -->
<define name="QUAD_NOPUSH_AIRSPEED" value="5.0"/> <!-- Maximum quadrotor without pusher motor airspeed -->
<define name="QUAD_MAX_AIRSPEED" value="12.0"/> <!-- Maximum quadrotor airspeed (with pusher motor)-->
<define name="QUAD_MAX_DECELERATION" value="0.75"/> <!-- Maximum horizontal deceleration in quad mode -->
<define name="SKEW_UP_AIRSPEED" value="10.0"/> <!-- Airspeed where the skewing starts when going up in airspeed -->
<define name="SKEW_DOWN_AIRSPEED" value="8.0"/> <!-- Airspeed where the skewing starts when going down in airspeed -->
<define name="SKEW_REF_MODEL" value="TRUE"/> <!-- Enable second order reference model for the skewing command -->
<define name="SKEW_REF_MODEL_P_GAIN" value="0.001"/> <!-- Skewing reference model proportional gain -->
<define name="SKEW_REF_MODEL_D_GAIN" value="0.003"/> <!-- Skewing reference model differential gain -->
<define name="SKEW_REF_MODEL_MAX_SPEED" value="20"/> <!-- Maximum rotational skewing speed bound for the reference model -->
<define name="FW_SKEW_ANGLE" value="85"/>
</section>
<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="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="INS_EXT_VISION_ROTATION" 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_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="MAG_CALIB" type="array">
<field type="struct">
<field name="abi_id" value="5"/>
<field name="calibrated" type="struct">
<field name="neutral" value="true"/>
<field name="scale" value="true"/>
<field name="rotation" value="true"/>
</field>
<field name="neutral" value="1757,3096,1119" type="int[]"/>
<field name="scale" value="{{10462,24574,6961},{18175,43831,12532}}"/>
<field name="body_to_sensor" value="{{0,16384,0,16384,0,0,0,0,-16384}}"/>
</field>
</define>
<define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-19,0,28}, .scale={{1537,43219,6232},{157,4410,641}}, .filter_sample_freq=1127, .filter_freq=30}, {.abi_id=21, .calibrated={.neutral=true, .scale=true},.neutral={-1,2,33}, .scale={{21914,8531,5489},{4477,1738,1120}}}, {.abi_id=22, .calibrated={.neutral=true, .scale=true},.neutral={-47,0,3}, .scale={{17288,29444,25808},{3531,6031,5293}}}}"/>
<define name="GYRO_CALIB" value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1127, .filter_freq=30}}"/>
<!-- Define axis in hover frame -->
<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>
<checklist>
<item name="pic" type="text">Enter the PIC</item>
<item name="pac" type="text">Enter the PAC</item>
<item name="gcs" type="text">Enter the GCS op</item>
<item name="goal" type="text">Goal of the flight</item>
<item name="basic law">Location, airspace and weather</item>
<item name="RC Battery">Check the RC battery</item>
<item name="tail connection">Check tail connection</item>
<item name="wing tape">Check wings taped and secured</item>
<item name="inspection">Inspect airframe condition</item>
<item name="attitude">Check attitude and heading</item>
<item name="airspeed">Airspeed sensor calibration</item>
<item name="takeoff location">Put UAV on take-off location</item>
<item name="flight plan">Check flight plan</item>
<item name="flight block">Switch to correct flight block</item>
<item name="drone tag">Switch on drone tag</item>
<item name="camera">Switch on camera</item>
<item name="announce">Announce flight to other airspace users</item>
</checklist>
<section PREFIX="ONELOOP_ANDI_" NAME="ONELOOP_ANDI">
<define name = "HEADING_MANUAL" value = "TRUE"/>
<define name = "YAW_STICK_IN_AUTO" value = "TRUE"/>
<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 = "{ 22.0f, 22.0f, 22.0f, 22.0f, 30.0f, 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="ROTWING_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/autopilot/rotorcraft_oneloop_switch.xml
+1 -1
View File
@@ -15,7 +15,7 @@
<include name="modules/radio_control/radio_control.h"/>
<include name="modules/gps/gps.h"/>
<include name="modules/actuators/actuators.h"/>
<include name="modules/rotwing_drone/rotwing_state_V2.h"/>
<include name="modules/rotwing_drone/rotwing_state.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"/>
conf/flight_plans/tudelft/oneloop_valkenburg.xml
+62 -48
View File
@@ -60,6 +60,11 @@
<corner name="p8"/>
</sector>
</sectors>
<variables>
<variable var="liftoff_pitch"/>
<variable var="liftoff_roll"/>
<variable var="stage_timer_msec"/>
</variables>
<modules>
<!--<module name="follow_me">
<define name="FOLLOW_ME_MOVING_WPS" value="WP_p1,WP_p2,WP_p3,WP_p4,WP_STDBY,WP_circ,WP_APP"/>
@@ -98,122 +103,131 @@
</exceptions>
<blocks>
<block name="Wait GPS">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<while cond="!GpsFixValid() || !state.ned_initialized_i"/>
</block>
<block name="Geo init" strip_button="Geo_init" strip_icon="googleearth.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<while cond="LessThan(NavBlockTime(), 10)"/>
<call_once fun="NavSetAltitudeReferenceHere()"/>
</block>
<block name="Holding point" strip_button="Holding point" strip_icon="off.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="NavKillThrottle()"/>
<set var="commands[COMMAND_MOTOR_PUSHER]" value="0"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
<block name="Start Engine" strip_button="Start Engines" strip_icon="on.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="0" roll="0" throttle="0" until="stage_time>10" vmode="throttle"/>
<exception cond="stage_time > 10" deroute="Holding point"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<call_once fun="NavResurrect()"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/>
</block>
<block name="Wait takeoff">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="stage_time > 10" deroute="Holding point"/>
<attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT 5.0) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT 5.0) @AND (stage_time > 2)" vmode="throttle"/>
</block>
<block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
<call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
<set var="stage_timer_msec" value="get_sys_time_msec()"/>
<set var="liftoff_roll" value="DegOfRad(stateGetNedToBodyEulers_f()->phi)"/>
<set var="liftoff_pitch" value="DegOfRad(stateGetNedToBodyEulers_f()->theta)"/>
<attitude pitch="liftoff_pitch" roll="liftoff_roll" throttle="0.75" until="(get_sys_time_msec()-stage_timer_msec)>250" vmode="throttle"/>
<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">
<block name="Climb">
<exception cond="GetPosHeight() @GT 20.0" deroute="Standby"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<call_once fun="nav_set_heading_current()"/>
<stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
</block>
<block name="Standby" strip_button="Standby" strip_icon="wp_quad.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_force_skew_off()"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="STDBY"/>
</block>
<block name="Standby_fwd" strip_button="Standby_fwd" strip_icon="wp_fwd.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
<call_once fun="rotwing_state_force_skew_off()"/>
<stay wp="STDBY"/>
<block name="Standby_free">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<stay wp="STDBY"/>
</block>
<block name="Descend" strip_button="Descend" strip_icon="descend.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_force_skew_off()"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="Descend"/>
</block>
<block name="safe" strip_button="Unsafe" strip_icon="alert.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<stay wp="HOME"/>
</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"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="nav.radius" wp="circ"/>
</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"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<circle radius="-nav.radius" wp="circ"/>
</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"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<circle radius="nav.radius" wp="circ"/>
</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"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<circle radius="-nav.radius" wp="circ"/>
</block>
<block name="route_fw" strip_button="Route_fwd" strip_icon="path.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_EFFICIENT)"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
<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)"/>
<block name="Transition_quad" strip_button="Transition Quad" strip_icon="wp_quad.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
<go wp="STDBY"/>
<deroute block="Standby"/>
</block>
<block name="land here">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<call_once fun="NavSetWaypointHere(WP_TD)"/>
</block>
<block name="land" strip_button="Land" strip_icon="land-right.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
<go wp="TD"/>
</block>
<block name="descend" strip_button="Descend" strip_icon="down.png">
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<block name="descend" strip_button="Descend" strip_icon="descend.png">
<call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_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)"/>
</block>
<block name="flare">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_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"/>
<block name="flare_low">
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<!-- <exception cond="NavDetectGround()" deroute="Holding point"/> -->
<exception cond="!nav_is_in_flight()" deroute="Holding point"/>
<exception cond="ground_detect()" deroute="Holding point"/>
<call_once fun="NavStartDetectGround()"/>
<!-- <call_once fun="NavStartDetectGround()"/> -->
<stay climb="-0.5" vmode="climb" wp="TD"/>
</block>
</block>
<block name="Manual" strip_button="Manual" strip_icon="joystick.png">
<set value="AP_MODE_ATTITUDE_DIRECT" var="autopilot_mode_auto2"/>
<stay wp="STDBY"/>
</block>
<block name="Landed">
<call_once fun="NavKillThrottle()"/>
<call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
<call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
<attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
</block>
</blocks>
</flight_plan>
conf/modules/eff_scheduling_rotwing.xml
+9 -1
View File
@@ -55,7 +55,7 @@
<define name="ROTWING_EFF_SCHED_IXX_WING" value="1"/>
<define name="ROTWING_EFF_SCHED_IYY_WING" value="1"/>
<define name="ROTWING_EFF_SCHED_M" value="1"/>
<define name="ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH" value="1"/>
<define name="ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH" value="{1,1}"/>
<define name="ROTWING_EFF_SCHED_DM_DPPRZ_HOVER_ROLL" value="{1,1}"/>
<define name="ROTWING_EFF_SCHED_HOVER_ROLL_PITCH_COEF" value="{1,1}"/>
<define name="ROTWING_EFF_SCHED_HOVER_ROLL_ROLL_COEF" value="{1,1}"/>
@@ -70,6 +70,14 @@
<define name="ROTWING_EFF_SCHED_K_LIFT_WING" value="{1,1}"/>
<define name="ROTWING_EFF_SCHED_K_LIFT_FUSELAGE" value="1"/>
<define name="ROTWING_EFF_SCHED_K_LIFT_TAIL" value="1"/>
<define name="GUIDANCE_INDI_HYBRID_USE_WLS" value="1"/>
<define name="GUIDANCE_INDI_HYBRID_U" value="9"/>
<define name="GUIDANCE_INDI_HYBRID_V" value="5"/>
<define name="GUIDANCE_INDI_WLS_PRIORITIES" value="{1000, 1000, 10, 100, 100}"/>
<define name="GUIDANCE_INDI_WLS_WU" value="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}"/>
<define name="GUIDANCE_INDI_MAX_PITCH" value="20.0"/>
<define name="GUIDANCE_INDI_MIN_PITCH" value="-20.0"/>
</test>
</makefile>
</module>
conf/modules/rotwing_state_V2.xml
-33
View File
@@ -1,33 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="rotwing_state_V2" dir="rotwing_drone">
<doc>
<description>This module keeps track of the current state of a rotating wing drone and desired state set by the RC or flightplan. Paramters are being scheduled in each change of a current state and desired state. Functions are defined in this module to call the actual state and desired state and set a desired state.</description>
<section name="ROTWING_STATE" prefix="ROTWING_STATE_">
<define name="USE_ROTATION_REF_MODEL" value="FALSE" description="Slow down the wing rotation with a reference model"/>
</section>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="RotWingState">
<dl_setting var="rotwing_state_skewing.wing_angle_deg_sp" min="0" step="1" max="90" shortname="skew angle"/>
<dl_setting var="rotwing_state_skewing.force_rotation_angle" min="0" step="1" max="1" values="FALSE|TRUE" shortname="force_skew"/>
<dl_setting var="demo_skew" min="0" step="1" max="1" values="FALSE|TRUE" shortname="demo_skew"/>
<dl_setting var="max_skew_demo" min="0" step="1" max="80" shortname="demo_max_skew"/>
<dl_setting var="min_skew_demo" min="0" step="1" max="80" shortname="demo_min_skew"/>
<dl_setting var="freq_skew_demo" min="0.1" step="0.1" max="10" shortname="demo_freq_skew"/>
<dl_setting var="rotwing_state_max_hover_speed" min="5" step="0.5" max="25" shortname="hover_speed"/>
<dl_setting var="hover_motors_active" min="0" step="1" max="1" values="FALSE|TRUE" shortname="h_motors_active"/>
<dl_setting var="bool_disable_hover_motors" min="0" step="1" max="1" values="FALSE|TRUE" shortname="h_motors_disable"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="rotwing_state_V2.h"/>
</header>
<init fun="init_rotwing_state()"/>
<periodic fun="periodic_rotwing_state()" freq="50"/>
<periodic fun="rotwing_state_skew_actuator_periodic()"/>
<makefile>
<file name="rotwing_state_V2.c"/>
</makefile>
</module>
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+1 -1
View File
@@ -411,7 +411,7 @@ void guidance_indi_enter(void)
init_butterworth_2_low_pass(&guidance_indi_airspeed_filt, tau_guidance_indi_airspeed, sample_time, 0.0);
}
void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed) {
void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed) {
gih_params.min_airspeed = min_airspeed;
gih_params.max_airspeed = max_airspeed;
}
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
+1 -1
View File
@@ -70,7 +70,7 @@ enum GuidanceIndiHybrid_VMode {
extern struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accep_sp, float heading_sp);
extern struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight, struct HorizontalGuidance *gh, struct VerticalGuidance *gv, enum GuidanceIndiHybrid_HMode h_mode, enum GuidanceIndiHybrid_VMode v_mode);
extern void guidance_indi_set_min_max_airspeed(float min_airspeed, float max_airspeed);
extern void guidance_set_min_max_airspeed(float min_airspeed, float max_airspeed);
struct guidance_indi_hybrid_params {
float pos_gain;
sw/airborne/modules/ctrl/eff_scheduling_rotwing.c
+93 -1
View File
@@ -29,7 +29,8 @@
#include "state.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
#include "modules/rotwing_drone/rotwing_state.h"
#include "autopilot.h"
#include "modules/actuators/actuators.h"
@@ -173,6 +174,8 @@ inline float bound_or_zero(float value, float low_lim, float up_lim) {
float eff_sched_pusher_time = 0.002;
float roll_eff_slider = 12.f;
static const float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU;
struct rotwing_eff_sched_var_t eff_sched_var;
inline void eff_scheduling_rotwing_update_wing_angle(void);
@@ -189,6 +192,7 @@ inline void eff_scheduling_rotwing_schedule_liftd(void);
inline float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED);
void stabilization_indi_set_wls_settings(void);
inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle);
/** ABI binding wing position data.
@@ -496,3 +500,91 @@ void stabilization_indi_set_wls_settings(void)
}
}
}
void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
{
// adjust weights
float fixed_wing_percentile = (rotwing_state_hover_motors_idling())? 1:0; // TODO: when hover props go below 40%, ...
Bound(fixed_wing_percentile, 0, 1);
#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16
float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES;
// Increase importance of forward acceleration in forward flight
wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile *
AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight)
struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
float du_min_thrust_z = ((MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ -
actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] +
(MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3]) * rotwing_state_hover_motors_running();
Bound(du_min_thrust_z, -50., 0.);
float du_max_thrust_z = -(actuator_state_filt_vect[0] * g1g2[3][0] + actuator_state_filt_vect[1] * g1g2[3][1] +
actuator_state_filt_vect[2] * g1g2[3][2] + actuator_state_filt_vect[3] * g1g2[3][3]);
Bound(du_max_thrust_z, 0., 50.);
float roll_limit_rad = guidance_indi_max_bank;
float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);
float fwd_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float quad_pitch_limit_rad = RadOfDeg(5.0);
float airspeed = stateGetAirspeed_f();
float scheduled_pitch_angle = 0.f;
float pitch_angle_range = 3.;
float meas_skew_angle = rotwing_state.meas_skew_angle_deg;
Bound(meas_skew_angle, 0, 90); // Bound to prevent errors
if (meas_skew_angle < ROTWING_SKEW_ANGLE_STEP) {
scheduled_pitch_angle = ROTWING_QUAD_PREF_PITCH;
wls_guid_p.Wu[1] = Wu_gih_original[1];
max_pitch_limit_rad = quad_pitch_limit_rad;
} else {
float pitch_progression = (airspeed - rotwing_state.fw_min_airspeed) / 2.f;
Bound(pitch_progression, 0.f, 1.f);
scheduled_pitch_angle = pitch_angle_range * pitch_progression + ROTWING_QUAD_PREF_PITCH*(1.f-pitch_progression);
wls_guid_p.Wu[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.99);
max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression;
}
if (!rotwing_state_hover_motors_running()) {
scheduled_pitch_angle = 8.;
max_pitch_limit_rad = fwd_pitch_limit_rad;
}
Bound(scheduled_pitch_angle, -5., 8.);
guidance_indi_pitch_pref_deg = scheduled_pitch_angle;
float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg);
// Set lower limits
wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle; //roll
wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle; // pitch
// Set upper limits limits
wls_guid_p.u_max[0] = roll_limit_rad - roll_angle; //roll
wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle; // pitch
if(rotwing_state_hover_motors_running()) {
wls_guid_p.u_min[2] = du_min_thrust_z;
wls_guid_p.u_max[2] = du_max_thrust_z;
} else {
wls_guid_p.u_min[2] = 0.;
wls_guid_p.u_max[2] = 0.;
}
if(rotwing_state_pusher_motor_running()) {
wls_guid_p.u_min[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]);
wls_guid_p.u_max[3] = 9.0; // Hacky value to prevent drone from pitching down in transition
} else {
wls_guid_p.u_min[3] = 0.;
wls_guid_p.u_max[3] = 0.;
}
// Set prefered states
wls_guid_p.u_pref[0] = 0; // prefered delta roll angle
wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle
wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; // Low thrust better for efficiency
wls_guid_p.u_pref[3] = body_v[0]; // solve the body acceleration
}
sw/airborne/modules/rotwing_drone/rotwing_state.c
+11 -114
View File
@@ -24,13 +24,12 @@
*/
#include "modules/rotwing_drone/rotwing_state.h"
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"
#include "firmwares/rotorcraft/autopilot_firmware.h"
#include "modules/core/commands.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"
/* Minimum measured RPM to consider the hover motors running (RPM) */
#ifndef ROTWING_QUAD_MIN_RPM
#define ROTWING_QUAD_MIN_RPM 400
@@ -61,11 +60,6 @@
#define ROTWING_FW_SKEW_ANGLE 85.0
#endif
/* Magnitude skew angle jump away from quad */
#ifndef ROTWING_SKEW_ANGLE_STEP
#define ROTWING_SKEW_ANGLE_STEP 55.0
#endif
/* TODO: Give a name.... */
#ifndef ROTWING_SKEW_BACK_MARGIN
#define ROTWING_SKEW_BACK_MARGIN 5.0
@@ -81,11 +75,6 @@
#define ROTWING_MIN_AIRSPEED_SLOPE_START_ANGLE 30.0
#endif
/* Preferred pitch angle for the quad mode (deg) */
#ifndef ROTWING_QUAD_PREF_PITCH
#define ROTWING_QUAD_PREF_PITCH -5.0
#endif
/* Amount of time the airspeed needs to be below the FW_MIN_AIRSPEED */
#ifndef ROTWING_FW_STALL_TIMEOUT
#define ROTWING_FW_STALL_TIMEOUT 0.5
@@ -107,13 +96,7 @@
#endif
abi_event rotwing_state_feedback_ev;
static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, uint8_t num_act);
static bool rotwing_state_hover_motors_idling(void);
static const float Wu_gih_original[GUIDANCE_INDI_HYBRID_U] = GUIDANCE_INDI_WLS_WU;
struct rotwing_state_t rotwing_state;
inline void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle);
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
@@ -164,7 +147,6 @@ void rotwing_state_init(void)
rotwing_state.fail_hover_motor = false;
rotwing_state.fail_pusher_motor = false;
rotwing_state.ref_model_skew_angle_deg = 0;
// Bind ABI messages
AbiBindMsgACT_FEEDBACK(ROTWING_STATE_ACT_FEEDBACK_ID, &rotwing_state_feedback_ev, rotwing_state_feedback_cb);
@@ -177,7 +159,7 @@ void rotwing_state_init(void)
* @brief Check if hover motors are idling (COMMAND_THRUST < ROTWING_QUAD_IDLE_MIN_THRUST) for ROTWING_QUAD_IDLE_TIMEOUT time
* @return true if hover motors are idling, false otherwise
*/
static bool rotwing_state_hover_motors_idling(void) {
bool rotwing_state_hover_motors_idling(void) {
static float last_idle_time = 0;
// Check if hover motors are idling and reset timer
if(stabilization.cmd[COMMAND_THRUST] > ROTWING_QUAD_IDLE_MIN_THRUST) {
@@ -306,9 +288,7 @@ void rotwing_state_periodic(void)
rotwing_state.max_airspeed = 0; // Max airspeed FW
}*/
guidance_indi_set_min_max_airspeed(rotwing_state.min_airspeed, rotwing_state.max_airspeed);
guidance_set_min_max_airspeed(rotwing_state.min_airspeed, rotwing_state.max_airspeed);
/* Set navigation/guidance settings */
nav_max_deceleration_sp = ROTWING_FW_MAX_DECELERATION * meas_skew_angle / 90.f + ROTWING_QUAD_MAX_DECELERATION * (90.f - meas_skew_angle) / 90.f; //TODO: Do we really want to based this on the skew?
@@ -333,13 +313,18 @@ void rotwing_state_periodic(void)
#else
rotwing_state.skew_cmd = servo_pprz_cmd;
#endif
#ifdef COMMAND_ROT_MECH
commands[COMMAND_ROT_MECH] = rotwing_state.skew_cmd;
#endif
/* Add simulation feedback for the skewing and RPM */
#if USE_NPS
// Export to the index of the SKEW in the NPS_ACTUATOR_NAMES array
#ifdef COMMAND_ROT_MECH
commands[COMMAND_ROT_MECH] = (rotwing_state.skew_cmd + MAX_PPRZ) / 2.f; // Scale to simulation command
#else
actuators_pprz[INDI_NUM_ACT] = (rotwing_state.skew_cmd + MAX_PPRZ) / 2.f; // Scale to simulation command
#endif
// SEND ABI Message to ctr_eff_sched, ourself and other modules that want Actuator position feedback
struct act_feedback_t feedback;
feedback.idx = SERVO_ROTATION_MECH_IDX;
@@ -448,94 +433,6 @@ bool rotwing_state_skew_angle_valid(void) {
return (!skew_timeout && skew_angle_match);
}
void guidance_indi_hybrid_set_wls_settings(float body_v[3], float roll_angle, float pitch_angle)
{
// adjust weights
float fixed_wing_percentile = (rotwing_state_hover_motors_idling())? 1:0; // TODO: when hover props go below 40%, ...
Bound(fixed_wing_percentile, 0, 1);
#define AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT 16
float Wv_original[GUIDANCE_INDI_HYBRID_V] = GUIDANCE_INDI_WLS_PRIORITIES;
// Increase importance of forward acceleration in forward flight
wls_guid_p.Wv[0] = Wv_original[0] * (1.0f + fixed_wing_percentile *
AIRSPEED_IMPORTANCE_IN_FORWARD_WEIGHT); // stall n low hover motor_off (weight 16x more important than vertical weight)
struct FloatEulers eulers_zxy;
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
float du_min_thrust_z = ((MAX_PPRZ - actuator_state_filt_vect[0]) * g1g2[3][0] + (MAX_PPRZ -
actuator_state_filt_vect[1]) * g1g2[3][1] + (MAX_PPRZ - actuator_state_filt_vect[2]) * g1g2[3][2] +
(MAX_PPRZ - actuator_state_filt_vect[3]) * g1g2[3][3]) * rotwing_state_hover_motors_running();
Bound(du_min_thrust_z, -50., 0.);
float du_max_thrust_z = -(actuator_state_filt_vect[0] * g1g2[3][0] + actuator_state_filt_vect[1] * g1g2[3][1] +
actuator_state_filt_vect[2] * g1g2[3][2] + actuator_state_filt_vect[3] * g1g2[3][3]);
Bound(du_max_thrust_z, 0., 50.);
float roll_limit_rad = guidance_indi_max_bank;
float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);
float fwd_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float quad_pitch_limit_rad = RadOfDeg(5.0);
float airspeed = stateGetAirspeed_f();
float scheduled_pitch_angle = 0.f;
float pitch_angle_range = 3.;
float meas_skew_angle = rotwing_state.meas_skew_angle_deg;
Bound(meas_skew_angle, 0, 90); // Bound to prevent errors
if (meas_skew_angle < ROTWING_SKEW_ANGLE_STEP) {
scheduled_pitch_angle = ROTWING_QUAD_PREF_PITCH;
wls_guid_p.Wu[1] = Wu_gih_original[1];
max_pitch_limit_rad = quad_pitch_limit_rad;
} else {
float pitch_progression = (airspeed - rotwing_state.fw_min_airspeed) / 2.f;
Bound(pitch_progression, 0.f, 1.f);
scheduled_pitch_angle = pitch_angle_range * pitch_progression + ROTWING_QUAD_PREF_PITCH*(1.f-pitch_progression);
wls_guid_p.Wu[1] = Wu_gih_original[1] * (1.f - pitch_progression*0.99);
max_pitch_limit_rad = quad_pitch_limit_rad + (fwd_pitch_limit_rad - quad_pitch_limit_rad) * pitch_progression;
}
if (!rotwing_state_hover_motors_running()) {
scheduled_pitch_angle = 8.;
max_pitch_limit_rad = fwd_pitch_limit_rad;
}
Bound(scheduled_pitch_angle, -5., 8.);
guidance_indi_pitch_pref_deg = scheduled_pitch_angle;
float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg);
// Set lower limits
wls_guid_p.u_min[0] = -roll_limit_rad - roll_angle; //roll
wls_guid_p.u_min[1] = min_pitch_limit_rad - pitch_angle; // pitch
// Set upper limits limits
wls_guid_p.u_max[0] = roll_limit_rad - roll_angle; //roll
wls_guid_p.u_max[1] = max_pitch_limit_rad - pitch_angle; // pitch
if(rotwing_state_hover_motors_running()) {
wls_guid_p.u_min[2] = du_min_thrust_z;
wls_guid_p.u_max[2] = du_max_thrust_z;
} else {
wls_guid_p.u_min[2] = 0.;
wls_guid_p.u_max[2] = 0.;
}
if(rotwing_state_pusher_motor_running()) {
wls_guid_p.u_min[3] = (-actuator_state_filt_vect[8] * g1g2[4][8]);
wls_guid_p.u_max[3] = 9.0; // Hacky value to prevent drone from pitching down in transition
} else {
wls_guid_p.u_min[3] = 0.;
wls_guid_p.u_max[3] = 0.;
}
// Set prefered states
wls_guid_p.u_pref[0] = 0; // prefered delta roll angle
wls_guid_p.u_pref[1] = -pitch_angle + pitch_pref_rad;// prefered delta pitch angle
wls_guid_p.u_pref[2] = wls_guid_p.u_max[2]; // Low thrust better for efficiency
wls_guid_p.u_pref[3] = body_v[0]; // solve the body acceleration
}
void rotwing_state_set(enum rotwing_states_t state) {
rotwing_state.nav_state = state;
}
sw/airborne/modules/rotwing_drone/rotwing_state.h
+11
View File
@@ -28,6 +28,16 @@
#include "std.h"
/* Magnitude skew angle jump away from quad */
#ifndef ROTWING_SKEW_ANGLE_STEP
#define ROTWING_SKEW_ANGLE_STEP 55.0
#endif
/* Preferred pitch angle for the quad mode (deg) */
#ifndef ROTWING_QUAD_PREF_PITCH
#define ROTWING_QUAD_PREF_PITCH -5.0
#endif
enum rotwing_states_t {
ROTWING_STATE_FORCE_HOVER,
ROTWING_STATE_REQUEST_HOVER,
@@ -84,6 +94,7 @@ void rotwing_state_periodic(void);
bool rotwing_state_hover_motors_running(void);
bool rotwing_state_pusher_motor_running(void);
bool rotwing_state_skew_angle_valid(void);
bool rotwing_state_hover_motors_idling(void);
void rotwing_state_set(enum rotwing_states_t state);
bool rotwing_state_choose_circle_direction(uint8_t wp_id);
sw/airborne/modules/rotwing_drone/rotwing_state_V2.c
-797
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/rotwing_drone/rotwing_state_V2.h
-102
View File
@@ -1,102 +0,0 @@
/*
* Copyright (C) 2023 Tomaso De Ponti <T.M.L.DePonti@tudelft.nl>
*
* This file is part of paparazzi
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/** @file "modules/rotwing/rotwing_state_V2.h"
* @author Tomaso De Ponti <T.M.L.DePonti@tudelft.nl>
* This module keeps track of the current state of a rotating wing drone and desired state set by the RC or flightplan. Paramters are being scheduled in each change of a current state and desired state. Functions are defined in this module to call the actual state and desired state and set a desired state.
*/
#ifndef ROTWING_STATE_H
#define ROTWING_STATE_H
#include "std.h"
/** Rotwing States
*/
#define ROTWING_STATE_HOVER 0 // Wing is skewed in 0 degrees (quad)
#define ROTWING_STATE_SKEWING 1 // WIng is skewing
#define ROTWING_STATE_FW 2 // Wing is skewed at 90 degrees (fixed wing), hover motors have full authority
#define ROTWING_STATE_FW_HOV_MOT_IDLE 3 // Wing is skewed at 90 degrees (fixed wing), hover motors are forced to idle
#define ROTWING_STATE_FW_HOV_MOT_OFF 4 // Wing is skewed at 90 degrees (fixed wubg), hover motors are switched off
#define ROTWING_STATE_FREE 5 // This is a desired state for which the controller has to decide the desired state itself
/** Rotwing Configurations
*/
#define ROTWING_CONFIGURATION_HOVER 0 // UAV is in hover
#define ROTWING_CONFIGURATION_HYBRID 1 // UAV can fly forward and hover if airspeed low, hover motors on
#define ROTWING_CONFIGURATION_EFFICIENT 2 // UAV flies efficiently forward (FW)
#define ROTWING_CONFIGURATION_FREE 3 // UAV switched between efficient and hybrid dependent on deceleration
struct RotwingState {
uint8_t current_state;
uint8_t desired_state;
uint8_t requested_config;
};
#define ROTWING_STATE_WING_QUAD_SETTING 0 // Wing skew at 0 degrees
#define ROTWING_STATE_WING_SCHEDULING_SETTING 1 // Wing skew handled by airspeed scheduler
#define ROTWING_STATE_WING_FW_SETTING 2 // Wing skew at 90 degrees
#define ROTWING_STATE_PITCH_QUAD_SETTING 0 // Pitch at prefered hover
#define ROTWING_STATE_PITCH_TRANSITION_SETTING 1 // Pitch scheduled
#define ROTWING_STATE_PITCH_FW_SETTING 2 // Pitch at prefered forward
struct RotWingStateSettings {
uint8_t wing_scheduler;
bool hover_motors_active;
bool hover_motors_disable;
bool force_forward;
uint8_t preferred_pitch_setting;
float preferred_pitch_value;
bool stall_protection;
float max_v_climb;
float max_v_descend;
float nav_max_speed;
};
struct RotWingStateSkewing {
float wing_angle_deg_sp; // Wing angle setpoint in deg
float wing_angle_deg; // Wing angle from sensor in deg
int32_t servo_pprz_cmd; // Wing rotation servo pprz cmd
bool airspeed_scheduling; // Airspeed scheduling on or off
bool force_rotation_angle; // Setting to force wing_angle_deg_sp
};
extern struct RotwingState rotwing_state;
extern struct RotWingStateSettings rotwing_state_settings;
extern struct RotWingStateSkewing rotwing_state_skewing;
extern float rotwing_state_max_hover_speed;
extern bool hover_motors_active;
extern bool bool_disable_hover_motors;
extern bool demo_skew;
extern float max_skew_demo;
extern float min_skew_demo;
extern float freq_skew_demo;
extern void init_rotwing_state(void);
extern void periodic_rotwing_state(void);
extern void request_rotwing_state(uint8_t state);
extern void rotwing_request_configuration(uint8_t configuration);
extern void rotwing_state_skew_actuator_periodic(void);
extern void rotwing_state_force_skew_off(void);
#endif // ROTWING_STATE_H