Skip to content

Commit

Permalink
Set wing rotation via rot_wing_state (#3177)
Browse files Browse the repository at this point in the history 
* Rotating wing controller moved to rotating wing state

extra airframe

* Update _IDX and rebase

* Requested changes from PR

* Code style

* xml fix

* Update sw/airborne/modules/ctrl/eff_scheduling_rot_wing.c
  • Loading branch information
@dewagter
dewagter committed Dec 13, 2023
1 parent a8f1c06 commit 250250d
Show file tree
Hide file tree
Showing 17 changed files with 899 additions and 465 deletions.
20 changes: 12 additions & 8 deletions conf/airframes/tudelft/rot_wing_25kg.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,10 @@
<define name="INS_EKF2_ACCEL_ID" value="IMU_CUBE1_ID"/>
<define name="INS_EKF2_MAG_ID" value="MAG_RM3100_SENDER_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"/>

<!-- Disable current sensing using UAVCAN -->
<define name="UAVCAN_ACTUATORS_USE_CURRENT" value="FALSE"/>
<define name="USE_I2C1"/>
Expand Down Expand Up @@ -139,7 +143,7 @@
<servo no="4" name="MOTOR_PUSH" min="-8191" neutral="-8191" max="8191"/>
<servo no="5" name="SERVO_ELEVATOR" min="-8191" neutral="-8191" max="4900"/>
<servo no="6" name="SERVO_RUDDER" min="-6500" neutral="0" max="6500"/>
<servo no="7" name="ROTATION_MECH" min="-8191" neutral="0" max="8191"/>
<servo no="7" name="ROTATION_MECH" min="8191" neutral="0" max="-8191"/>
<servo no="8" name="AIL_RIGHT" min="-8000" neutral="-1000" max="5000"/>
<servo no="9" name="FLAP_RIGHT" min="-8000" neutral="-1000" max="5000"/>
<servo no="10" name="AIL_LEFT" min="-5000" neutral="1000" max="8000"/>
Expand Down Expand Up @@ -187,7 +191,7 @@
<set servo="MOTOR_PUSH" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="SERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/>
<set servo="SERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/>
<set servo="ROTATION_MECH" value="wing_rotation_controller.servo_pprz_cmd"/>
<set servo="ROTATION_MECH" value="rotwing_state_skewing.servo_pprz_cmd"/>
<set servo="AIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="FLAP_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/>
<set servo="AIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
Expand All @@ -202,7 +206,7 @@
<set servo="BMOTOR_PUSH" value="($th_hold? -9600 : actuators_pprz[8])"/>
<set servo="BSERVO_ELEVATOR" value="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))"/>
<set servo="BSERVO_RUDDER" value="($servo_hold? RadioControlValues(RADIO_YAW) : (!autopilot_in_flight()? 0 : actuators_pprz[4]))"/>
<set servo="BROTATION_MECH" value="wing_rotation_controller.servo_pprz_cmd"/>
<set servo="BROTATION_MECH" value="rotwing_state_skewing.servo_pprz_cmd"/>
<set servo="BAIL_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
<set servo="BFLAP_RIGHT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/>
<set servo="BAIL_LEFT" value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
Expand All @@ -212,7 +216,7 @@


<section PREFIX="SYS_ID_" NAME="SYS_ID">
<define name="DOUBLET_AXES" value="{0,1,2,3,4,5,6}"/>
<define name="DOUBLET_AXES" value="{0,1,2,3,4,5,6,7,8}"/>
<define name="DOUBLET_RADIO_CHANNEL" value="6"/>

<define name="AUTO_DOUBLETS_N_ACTUATORS" value="4"/>
Expand Down Expand Up @@ -278,8 +282,8 @@
<define name="USE_GROUND_DETECT_INDI_THRUST" value="TRUE"/>
<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_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>

Expand Down Expand Up @@ -373,8 +377,8 @@
<define name="REF_ERR_P" value="40.0"/>
<define name="REF_ERR_Q" value="32.0"/>
<define name="REF_ERR_R" value="23.0"/>
<define name="REF_RATE_P" value="7.0"/>
<define name="REF_RATE_Q" value="7.2"/>
<define name="REF_RATE_P" value="5.5"/>
<define name="REF_RATE_Q" value="5.5"/>
<define name="REF_RATE_R" value="3.9"/>

<define name="MAX_R" value="30.0" unit="deg/s"/>
Expand Down
45 changes: 19 additions & 26 deletions conf/airframes/tudelft/rot_wing_v3b.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,7 +119,7 @@
<!-- Other -->
<module name="sys_id_doublet"/>
<module name="sys_id_auto_doublets"/>
<module name="wing_rotation_controller_servo"/>
<module name="wing_rotation_adc_sensor"/>
<module name="rot_wing_automation"/>
<module name="ground_detect_sensor"/>
<module name="rotwing_state"/>
Expand Down Expand Up @@ -189,7 +189,7 @@
<!-- PWM actuators -->
<servos driver="Pwm">
<!--1018 = quad 2086 = fw-->
<servo NO="0" NAME="ROTATION_MECH" MIN="1018" NEUTRAL="1018" MAX="2086"/>
<servo NO="0" NAME="ROTATION_MECH" MIN="1018" NEUTRAL="1552" MAX="2086"/>
</servos>

<!-- Can bus 1 actuators -->
Expand Down Expand Up @@ -240,7 +240,7 @@
<set VALUE="($servo_hold? RadioControlValues(RADIO_YAW) : actuators_pprz[4])" SERVO="SERVO_RUDDER"/>
<set VALUE="($servo_hold? (RadioControlValues(RADIO_PITCH)/4+7200) : (!autopilot_in_flight()? 0 : actuators_pprz[5]))" SERVO="SERVO_ELEVATOR"/>
<set VALUE="($th_hold? -9600 : actuators_pprz[8])" SERVO="MOTOR_PUSH"/>
<set VALUE="wing_rotation_controller.servo_pprz_cmd" SERVO="ROTATION_MECH"/>
<set VALUE="rotwing_state_skewing.servo_pprz_cmd" SERVO="ROTATION_MECH"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_LEFT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])" SERVO="AIL_RIGHT"/>
<set VALUE="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])" SERVO="FLAP_LEFT"/>
Expand All @@ -254,7 +254,7 @@
</command_laws>

<section PREFIX="SYS_ID_" NAME="SYS_ID">
<define name="DOUBLET_AXES" value="{0,1,2,3,4,5,6}"/>
<define name="DOUBLET_AXES" value="{0,1,2,3,4,5,6,7,8}"/>
<define name="DOUBLET_RADIO_CHANNEL" value="6"/>

<define name="AUTO_DOUBLETS_N_ACTUATORS" value="4"/>
Expand Down Expand Up @@ -299,16 +299,17 @@
<define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * 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="200"/>
<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="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<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="200"/>
<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="NAV_HYBRID_MAX_DECELERATION" value="0.5"/>
<define name="ROTWING_STATE_USE_ROTATION_REF_MODEL" value="TRUE"/>
</section>

<section name="GROUND_DETECT">
Expand Down Expand Up @@ -384,9 +385,9 @@
<define name="G2" value="{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}"/>

<!-- Actuator dynamics -->
<define name="ACT_DYN" value="{0.02, 0.02, 0.02, 0.02, 0.1, 0.1, 0.1, 0.1, 0.024}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>
<define name="ACT_FREQ" value="{10.1, 10.1, 10.1, 10.1, 52.7, 52.7, 52.7, 52.7, 24.1}"/>
<define name="ACT_IS_SERVO" value="{ 0, 0, 0, 0, 1, 1, 1, 1, 0}"/>
<define name="ACT_IS_THRUSTER_X" value="{ 0, 0, 0, 0, 0, 0, 0, 0, 1}"/>

<!-- Reference -->
<define name="REF_ERR_P" value="40.0"/>
Expand Down Expand Up @@ -416,14 +417,6 @@
<define name="ROTWING_V3B" value="1" />
</section>

<section name="WING_ROTATION" prefix="WING_ROTATION_CONTROLLER_">
<define name="POSITION_ADC_0" value="62930"/>
<define name="POSITION_ADC_90" value="41760"/>
<define name="DEADZONE_PPRZ_CMD" value="500"/>
<define name="P_GAIN" value="-50000"/>
<define name="MAX_CMD" value="9600"/>
</section>

<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
<!-- Gains -->
<define name="HOVER_KP" value="310"/>
Expand All @@ -450,7 +443,7 @@

<section name="GUIDANCE_INDI_HYBRID" prefix="GUIDANCE_INDI_">
<!--WLS settings-->
<define name="USE_WLS" value="FALSE|TRUE"/>
<define name="USE_WLS" value="TRUE"/>
<define name="WLS_PRIORITIES" value="{10., 10., 10.}"/>
<define name="WLS_WU" value="{10., 10., 100., 1.}"/>

Expand Down

0 comments on commit 250250d

Please sign in to comment.