Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 12:57:27 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Set wing rotation via rot_wing_state (#3177)

Browse Source 
* 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
This commit is contained in:
Christophe De Wagter
2023-12-13 14:23:52 +01:00
committed by GitHub
parent a8f1c063a6
commit 250250d04c
17 changed files with 890 additions and 456 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/tudelft/rot_wing_25kg.xml
+12 -8
View File
@@ -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"/>
@@ -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"/>
@@ -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])"/>
@@ -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])"/>
@@ -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"/>
@@ -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>
@@ -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"/>
conf/airframes/tudelft/rot_wing_v3b.xml
+19 -26
View File
@@ -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"/>
@@ -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 -->
@@ -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"/>
@@ -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"/>
@@ -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">
@@ -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"/>
@@ -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"/>
@@ -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.}"/>
conf/airframes/tudelft/rot_wing_v3d.xml
+516
View File
File diff suppressed because it is too large Load Diff
conf/modules/eff_scheduling_rot_wing.xml
+1 -1
View File
@@ -27,7 +27,7 @@
<file name="eff_scheduling_rot_wing.c"/>
<test>
<define name="ACTUATORS_NB" value="10"/>
<define name="SERVO_ROTATION_MECH" value="8"/>
<define name="SERVO_ROTATION_MECH_IDX" value="8"/>
<define name="SERVO_MOTOR_FRONT" value="0"/>
<define name="SERVO_MOTOR_RIGHT" value="1"/>
<define name="SERVO_MOTOR_BACK" value="2"/>
conf/modules/rotwing_state.xml
+4
View File
@@ -2,6 +2,9 @@
<module name="rotwing_state" dir="rot_wing_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>
@@ -19,6 +22,7 @@
</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.c"/>
</makefile>
conf/modules/sys_id_doublet.xml
+1 -1
View File
@@ -37,7 +37,7 @@
<dl_setting min="0" max="1" step="1" values="Inactive|Active" shortname="Doublet" var="doublet_active" type="uint8_t" module="system_identification/sys_id_doublet" handler="activate_handler"/>
<dl_setting min="0" max="2" step="1" values="Normal|Half|3211" shortname="mode" var="doublet_mode" type="uint8_t" module="system_identification/sys_id_doublet" handler="mod_handler" param="SYS_ID_DOUBLET_MOD"/>
<dl_setting min="-9600" max="9600" step="100" shortname="Amplitude" var="doublet_amplitude" type="int32_t" module="system_identification/sys_id_doublet"/>
<dl_setting min="0" max="7" step="1" shortname="Doublet axis" var="doublet_axis" type="uint8_t" module="system_identification/sys_id_doublet" handler="axis_handler"/>
<dl_setting min="0" max="8" step="1" shortname="Doublet axis" var="doublet_axis" type="uint8_t" module="system_identification/sys_id_doublet" handler="axis_handler"/>
<dl_setting min="0" max="100" step="0.5" shortname="Length_s" var="doublet_length_s" type="float" module="system_identification/sys_id_doublet"/>
<dl_setting min="0" max="100" step="0.5" shortname="Extra_waiting_s" var="doublet_extra_waiting_time_s" type="float" module="system_identification/sys_id_doublet"/>
</dl_settings>
conf/modules/wing_rotation_adc_sensor.xml
+18
View File
@@ -0,0 +1,18 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="wing_rotation_adc_sensor" dir="rot_wing_drone">
<doc>
<description>Module to read wing skew angle from ADC</description>
<configure name="ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION" default="ADC_5" description="ADC channel connected to the wing rotation potentiometer"/>
</doc>
<header>
<file name="wing_rotation_adc_sensor.h"/>
</header>
<init fun="wing_rotation_adc_init()"/>
<periodic fun="wing_rotation_adc_to_deg()"/>
<makefile>
<configure name="ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION" default="ADC_5" case="lower|upper"/>
<define name="ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION" value="$(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_UPPER)"/>
<define name="USE_$(ADC_CHANNEL_WING_ROTATION_CONTROLLER_POSITION_UPPER)"/>
<file name="wing_rotation_adc_sensor.c"/>
</makefile>
</module>
conf/telemetry/highspeed_rotorcraft.xml
+3 -1
View File
@@ -254,7 +254,9 @@
<message name="IMU_HEATER" period="1.0"/>
<message name="AIRSPEED_RAW" period="0.01"/>
<message name="EFF_MAT_G" period="0.002"/>
<message name="GUIDANCE" period="0.002"/>
<message name="GUIDANCE" period="0.002"/>
<message name="ROTATING_WING_STATE" period="0.1"/>
<message name="DOUBLET" period="0.002"/>
</mode>
</process>
conf/userconf/tudelft/conf.xml
+13 -2
View File
@@ -139,7 +139,7 @@
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/tudelft/cyclone_valkenburg.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="#ffff7f7f0000"
/>
<aircraft
@@ -579,7 +579,7 @@
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.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/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.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 modules/wing_rotation_controller_servo.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.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/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.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"
/>
<aircraft
@@ -604,4 +604,15 @@
settings_modules="modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
gui_color="red"
/>
<aircraft
name="RotatingWingV3d"
ac_id="23"
airframe="airframes/tudelft/rot_wing_v3d.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ekf_aw.xml modules/electrical.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/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"
/>
</conf>