27 changes: 27 additions & 0 deletions conf/modules/wing_rotation_controller_v3b.xml
View file
@@ -0,0 +1,27 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="wing_rotation_controller_v3b" dir="rot_wing_drone">
<doc>
<description>Module to control wing rotation servo command based on prefered angle setpoint</description>
</doc>
<settings>
<dl_settings NAME="wing_rotation">
<dl_settings NAME="wing_rot">
<dl_setting MAX="90" MIN="0" STEP="1" VAR="wing_rotation.wing_angle_deg_sp" shortname="wing_sp_deg"/>
<dl_setting MAX="1" MIN="0" STEP="1" VAR="wing_rotation.airspeed_scheduling" shortname="airspeed_sched"/>
<dl_setting MAX="18" MIN="14" STEP="0.1" VAR="wing_rotation.forward_airspeed" shortname="forward_airspeed"/>
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation.wing_rotation_first_order_dynamics" shortname="first_dyn"/>
<dl_setting MAX="0.01" MIN="0" STEP="0.0001" VAR="wing_rotation.wing_rotation_second_order_dynamics" shortname="second_dyn"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="wing_rotation_controller_v3b.h"/>
</header>
<init fun="wing_rotation_init()"/>
<periodic fun="wing_rotation_periodic()" freq="1.0"/>
<periodic fun="wing_rotation_event()"/>
<makefile>
<file name="wing_rotation_controller_v3b.c"/>
<test/>
</makefile>
</module>
2 changes: 1 addition & 1 deletion conf/radios/crossfire_sbus.xml
View file
Expand Up @@ -9,7 +9,7 @@
<channel function="AUX1" min="1100" neutral="1500" max="1900" average="1"/> <!-- TH_HOLD -->
<channel function="AUX2" min="1100" neutral="1500" max="1900" average="1"/> <!-- FMODE -->
<channel function="AUX3" min="1100" neutral="1500" max="1900" average="1"/> <!-- FBW_MODE -->
<channel function="AUX4" min="1100" neutral="1500" max="1900" average="1"/>
<channel function="AUX4" min="1100" neutral="1100" max="1900" average="1"/>
<channel function="AUX5" min="1100" neutral="1500" max="1900" average="1"/>
<channel function="AUX6" min="1100" neutral="1500" max="1900" average="1"/>
<channel function="AUX7" min="1100" neutral="1500" max="1900" average="1"/>
Expand Down
448 changes: 448 additions & 0 deletions conf/simulator/jsbsim/aircraft/rotwing25.xml
View file

Large diffs are not rendered by default.

259 changes: 259 additions & 0 deletions conf/simulator/jsbsim/aircraft/rotwingv3.xml
View file
@@ -0,0 +1,259 @@
<?xml version="1.0"?>
<?xml-stylesheet type="text/xsl" href="http://jsbsim.sourceforge.net/JSBSim.xsl"?>
<fdm_config name="QUAD COMPLETE EXT" version="2.0" release="BETA" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="http://jsbsim.sourceforge.net/JSBSim.xsd">

<fileheader>
<author>Dennis van Wijngaarden </author>
<filecreationdate>08-04-2022</filecreationdate>
<version>Version 0.9 - beta</version>
<description>Rotating wing drone v3 with actuator dynamics (NE/SW turning CCW, NW/SE CW)</description>
</fileheader>

<metrics>
<wingarea unit="IN2"> 78.53 </wingarea>
<wingspan unit="IN"> 10 </wingspan>
<chord unit="IN"> 6.89 </chord>
<htailarea unit="FT2"> 0 </htailarea>
<htailarm unit="FT"> 0 </htailarm>
<vtailarea unit="FT2"> 0 </vtailarea>
<vtailarm unit="FT"> 0 </vtailarm>
<location name="AERORP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="EYEPOINT" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
<location name="VRP" unit="IN">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</metrics>

<mass_balance>
<ixx unit="SLUG*FT2"> 0.047772 </ixx>
<iyy unit="SLUG*FT2"> 0.054670 </iyy>
<izz unit="SLUG*FT2"> 1.203701 </izz>
<ixy unit="SLUG*FT2"> 0. </ixy>
<ixz unit="SLUG*FT2"> 0. </ixz>
<iyz unit="SLUG*FT2"> 0. </iyz>
<emptywt unit="LBS"> 14.52846 </emptywt>
<location name="CG" unit="M">
<x> 0 </x>
<y> 0 </y>
<z> 0 </z>
</location>
</mass_balance>

<ground_reactions>
<contact type="STRUCTURE" name="CONTACT_FRONT">
<location unit="M">
<x>-0.15 </x>
<y> 0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>

<contact type="STRUCTURE" name="CONTACT_BACK">
<location unit="M">
<x> 0.15</x>
<y> 0 </y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>

<contact type="STRUCTURE" name="CONTACT_RIGHT">
<location unit="M">
<x> 0. </x>
<y> 0.15</y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>

<contact type="STRUCTURE" name="CONTACT_LEFT">
<location unit="M">
<x> 0. </x>
<y>-0.15</y>
<z>-0.1 </z>
</location>
<static_friction> 0.8 </static_friction>
<dynamic_friction> 0.5 </dynamic_friction>
<spring_coeff unit="N/M"> 500 </spring_coeff>
<damping_coeff unit="N/M/SEC"> 100 </damping_coeff>
<damping_coeff_rebound type="SQUARE" unit="N/M2/SEC2"> 1000 </damping_coeff_rebound>
<max_steer unit="DEG"> 0.0 </max_steer>
<brake_group> NONE </brake_group>
<retractable>0</retractable>
</contact>
</ground_reactions>

<flight_control name="actuator_dynamics">
<channel name="filtering">

<!--First order filter represents actuator dynamics-->
<lag_filter name="front_motor_lag">
<input> fcs/front_motor </input>
<c1> 50 </c1>
<output> fcs/front_motor_lag</output>
</lag_filter>
<lag_filter name="right_motor_lag">
<input> fcs/right_motor </input>
<c1> 50 </c1>
<output> fcs/right_motor_lag</output>
</lag_filter>
<lag_filter name="back_motor_lag">
<input> fcs/back_motor </input>
<c1> 50 </c1>
<output> fcs/back_motor_lag</output>
</lag_filter>
<lag_filter name="left_motor_lag">
<input> fcs/left_motor </input>
<c1> 50 </c1>
<output> fcs/left_motor_lag</output>
</lag_filter>
</channel>

</flight_control>

<external_reactions>

<property>fcs/front_motor</property>
<property>fcs/front_motor_lag</property>
<property>fcs/right_motor</property>
<property>fcs/right_motor_lag</property>
<property>fcs/back_motor</property>
<property>fcs/back_motor_lag</property>
<property>fcs/left_motor</property>
<property>fcs/left_motor_lag</property>


<!-- First the lift forces produced by each propeller -->

<force name="front_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/front_motor_lag</property>
<value> 7.0 </value>
</product>
</function>
<location unit="IN">
<x>-18.504</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>

<force name="right_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/right_motor_lag</property>
<value> 7.0 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>14.567</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>

<force name="back_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/back_motor_lag</property>
<value> 7.0 </value>
</product>
</function>
<location unit="IN">
<x>18.504</x>
<y>0</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>

<force name="left_motor" frame="BODY" unit="LBS">
<function>
<product>
<property>fcs/left_motor_lag</property>
<value> 7.0 </value>
</product>
</function>
<location unit="IN">
<x>0</x>
<y>-14.567</y>
<z>0</z>
</location>
<direction>
<x>0</x>
<y>0</y>
<z>-1</z>
</direction>
</force>

<!-- Then the Moment Couples -->


</external_reactions>

<propulsion/>

<flight_control name="FGFCS"/>

<aerodynamics>
<axis name="DRAG">
<function name="aero/coefficient/CD">
<description>Drag</description>
<product>
<property>aero/qbar-psf</property>
<value>47.9</value> <!-- Conversion to pascals -->
<value>0.0151</value> <!-- CD x Area (m^2) -->
<value>0.224808943</value> <!-- N to LBS -->
</product>
</function>
</axis>
</aerodynamics>

</fdm_config>
448 changes: 448 additions & 0 deletions conf/simulator/jsbsim/aircraft/rotwingv3b.xml
View file

Large diffs are not rendered by default.

939 changes: 939 additions & 0 deletions conf/simulator/jsbsim/aircraft/rotwingv3b_fwd.xml
View file

Large diffs are not rendered by default.

40 changes: 32 additions & 8 deletions conf/telemetry/highspeed_rotorcraft.xml
View file
Expand Up @@ -47,6 +47,17 @@
<message name="AHRS_REF_QUAT" period="0.1"/>
<message name="INS_FLOW_INFO" period="0.1"/>
<message name="GPS_RTK" period="0.1"/>
<message name="STAB_ATTITUDE_INDI" period="0.05"/>
<message name="ROT_WING_CONTROLLER" period="0.2"/>
<message name="AOA" period="0.2"/>
<message name="ACTUATORS" period="0.02"/>
<message name="INDI_G" period="0.2"/>
<message name="APPROACH_MOVING_TARGET" period="0.1"/>
<message name="AIRSPEED_RAW" period="0.1"/>
<message name="TARGET_POS_INFO" period="0.1"/>
<message name="THRUST_BX_EFF" period="0.1"/>
<message name="ROTATING_WING_STATE" period="0.2"/>
<message name="ROTORCRAFT_CMD" period="0.2"/>
</mode>

<mode name="calibration">
Expand Down Expand Up @@ -74,14 +85,16 @@
</mode>

<mode name="raw_sensors">
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="2.1"/>
<message name="IMU_ACCEL_RAW" period=".05"/>
<message name="IMU_GYRO_RAW" period=".05"/>
<message name="IMU_MAG_RAW" period=".05"/>
<message name="BARO_RAW" period=".1"/>
<message name="IMU_MAG_CURRENT_CALIBRATION" period=".05"/>
<message name="ARDRONE_NAVDATA" period=".05"/>
<message name="AIRSPEED_RAW" period="0.1"/>
</mode>

<mode name="scaled_sensors">
Expand Down Expand Up @@ -149,6 +162,7 @@
<mode name="vel_guidance" key_press="q">
<message name="ALIVE" period="0.9"/>
<message name="HYBRID_GUIDANCE" period="0.062"/>
<message name="GUIDANCE_INDI_HYBRID" period="0.1"/>
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
<message name="ROTORCRAFT_FP" period="0.8"/>
<message name="ROTORCRAFT_STATUS" period="1.2"/>
Expand Down Expand Up @@ -242,6 +256,16 @@
<message name="ACTUATORS" period="0.002"/>
<message name="GPS_RTK" period="0.1"/>
<message name="IMU_HEATER" period="1.0"/>
<message name="ROT_WING_CONTROLLER" period="0.2"/>
<message name="AOA" period="0.02"/>
<message name="DOUBLET" period="0.002"/>
<message name="APPROACH_MOVING_TARGET" period="0.1"/>
<message name="TARGET_POS_INFO" period="0.1"/>
<message name="AIRSPEED_RAW" period="0.002"/>
<message name="AIRSPEED_WIND_ESTIMATOR_EKF" period="0.04"/>
<message name="AIRSPEED_WIND_ESTIMATOR_EKF_COV" period="0.04"/>
<message name="AIRSPEED_WIND_ESTIMATOR_EKF_FORCES" period="0.04"/>
<message name="ROTATING_WING_STATE" period="0.1"/>
</mode>
</process>

Expand Down
22 changes: 22 additions & 0 deletions conf/userconf/tudelft/conf.xml
View file
Expand Up @@ -571,4 +571,26 @@
settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml"
gui_color="red"
/>
<aircraft
name="RotatingWingV3b"
ac_id="39"
airframe="airframes/tudelft/rot_wing_v3b.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/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/ctrl_eff_sched_rot_wing_v3b.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/rot_wing_automation.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml modules/wing_rotation_controller_v3b.xml"
gui_color="red"
/>
<aircraft
name="rot_wing_25kg"
ac_id="29"
airframe="airframes/tudelft/rot_wing_25kg.xml"
radio="radios/crossfire_sbus.xml"
telemetry="telemetry/highspeed_rotorcraft.xml"
flight_plan="flight_plans/tudelft/rotating_wing25kg_EHVB.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/actuators_faulhaber.xml modules/air_data.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_rot_wing.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/rot_wing_auto_doublet.xml modules/stabilization_indi.xml modules/sys_id_doublet.xml"
gui_color="red"
/>
</conf>
4 changes: 2 additions & 2 deletions sw/airborne/boards/cube/orange/board.cfg
View file
Expand Up @@ -140,8 +140,8 @@ PD09 UART3_RX UART AF:USART3_RX # TELEM2
PD10 SPI_SLAVE7 OUTPUT PUSHPULL SPEED_HIGH FLOATING LEVEL_HIGH # INV FRAM_CS
PD11 UART3_CTS PASSIVE # TELEM2
PD12 UART3_RTS PASSIVE # TELEM2
PD13 SERVO5 PWM AF:TIM4_CH2 ()
PD14 SERVO6 PWM AF:TIM4_CH3 ()
PD13 PWM_INPUT1 ICU AF:TIM4_CH2 () # SERVO5
PD14 PWM_INPUT2 ICU AF:TIM4_CH3 () # SERVO6
PD15 MPU_DRDY PASSIVE

PE00 UART8_RX UART AF:UART8_RX # GPS2
Expand Down
60 changes: 29 additions & 31 deletions sw/airborne/boards/cube/orange/board.h
View file
Expand Up @@ -125,8 +125,8 @@
#define PD10_SPI_SLAVE7 10U
#define PD11_UART3_CTS 11U
#define PD12_UART3_RTS 12U
#define PD13_SERVO5 13U
#define PD14_SERVO6 14U
#define PD13_PWM_INPUT1 13U
#define PD14_PWM_INPUT2 14U
#define PD15_MPU_DRDY 15U

#define PE00_UART8_RX 0U
Expand Down Expand Up @@ -313,8 +313,8 @@
#define LINE_SPI_SLAVE7 PAL_LINE(GPIOD, 10U)
#define LINE_UART3_CTS PAL_LINE(GPIOD, 11U)
#define LINE_UART3_RTS PAL_LINE(GPIOD, 12U)
#define LINE_SERVO5 PAL_LINE(GPIOD, 13U)
#define LINE_SERVO6 PAL_LINE(GPIOD, 14U)
#define LINE_PWM_INPUT1 PAL_LINE(GPIOD, 13U)
#define LINE_PWM_INPUT2 PAL_LINE(GPIOD, 14U)
#define LINE_MPU_DRDY PAL_LINE(GPIOD, 15U)

#define LINE_UART8_RX PAL_LINE(GPIOE, 0U)
Expand Down Expand Up @@ -682,8 +682,8 @@
PIN_MODE_OUTPUT(PD10_SPI_SLAVE7) | \
PIN_MODE_INPUT(PD11_UART3_CTS) | \
PIN_MODE_INPUT(PD12_UART3_RTS) | \
PIN_MODE_ALTERNATE(PD13_SERVO5) | \
PIN_MODE_ALTERNATE(PD14_SERVO6) | \
PIN_MODE_ALTERNATE(PD13_PWM_INPUT1) | \
PIN_MODE_ALTERNATE(PD14_PWM_INPUT2) | \
PIN_MODE_INPUT(PD15_MPU_DRDY))

#define VAL_GPIOD_OTYPER (PIN_OTYPE_PUSHPULL(PD00_CAN1_RX) | \
Expand All @@ -699,8 +699,8 @@
PIN_OTYPE_PUSHPULL(PD10_SPI_SLAVE7) | \
PIN_OTYPE_OPENDRAIN(PD11_UART3_CTS) | \
PIN_OTYPE_OPENDRAIN(PD12_UART3_RTS) | \
PIN_OTYPE_PUSHPULL(PD13_SERVO5) | \
PIN_OTYPE_PUSHPULL(PD14_SERVO6) | \
PIN_OTYPE_PUSHPULL(PD13_PWM_INPUT1) | \
PIN_OTYPE_PUSHPULL(PD14_PWM_INPUT2) | \
PIN_OTYPE_OPENDRAIN(PD15_MPU_DRDY))

#define VAL_GPIOD_OSPEEDR (PIN_OSPEED_SPEED_HIGH(PD00_CAN1_RX) | \
Expand All @@ -716,8 +716,8 @@
PIN_OSPEED_SPEED_HIGH(PD10_SPI_SLAVE7) | \
PIN_OSPEED_SPEED_VERYLOW(PD11_UART3_CTS) | \
PIN_OSPEED_SPEED_VERYLOW(PD12_UART3_RTS) | \
PIN_OSPEED_SPEED_HIGH(PD13_SERVO5) | \
PIN_OSPEED_SPEED_HIGH(PD14_SERVO6) | \
PIN_OSPEED_SPEED_HIGH(PD13_PWM_INPUT1) | \
PIN_OSPEED_SPEED_HIGH(PD14_PWM_INPUT2) | \
PIN_OSPEED_SPEED_VERYLOW(PD15_MPU_DRDY))

#define VAL_GPIOD_PUPDR (PIN_PUPDR_FLOATING(PD00_CAN1_RX) | \
Expand All @@ -733,8 +733,8 @@
PIN_PUPDR_FLOATING(PD10_SPI_SLAVE7) | \
PIN_PUPDR_PULLDOWN(PD11_UART3_CTS) | \
PIN_PUPDR_PULLDOWN(PD12_UART3_RTS) | \
PIN_PUPDR_FLOATING(PD13_SERVO5) | \
PIN_PUPDR_FLOATING(PD14_SERVO6) | \
PIN_PUPDR_FLOATING(PD13_PWM_INPUT1) | \
PIN_PUPDR_FLOATING(PD14_PWM_INPUT2) | \
PIN_PUPDR_PULLDOWN(PD15_MPU_DRDY))

#define VAL_GPIOD_ODR (PIN_ODR_LEVEL_HIGH(PD00_CAN1_RX) | \
Expand All @@ -750,8 +750,8 @@
PIN_ODR_LEVEL_HIGH(PD10_SPI_SLAVE7) | \
PIN_ODR_LEVEL_HIGH(PD11_UART3_CTS) | \
PIN_ODR_LEVEL_HIGH(PD12_UART3_RTS) | \
PIN_ODR_LEVEL_LOW(PD13_SERVO5) | \
PIN_ODR_LEVEL_LOW(PD14_SERVO6) | \
PIN_ODR_LEVEL_HIGH(PD13_PWM_INPUT1) | \
PIN_ODR_LEVEL_HIGH(PD14_PWM_INPUT2) | \
PIN_ODR_LEVEL_HIGH(PD15_MPU_DRDY))

#define VAL_GPIOD_AFRL (PIN_AFIO_AF(PD00_CAN1_RX, 9) | \
Expand All @@ -768,8 +768,8 @@
PIN_AFIO_AF(PD10_SPI_SLAVE7, 0) | \
PIN_AFIO_AF(PD11_UART3_CTS, 0) | \
PIN_AFIO_AF(PD12_UART3_RTS, 0) | \
PIN_AFIO_AF(PD13_SERVO5, 2) | \
PIN_AFIO_AF(PD14_SERVO6, 2) | \
PIN_AFIO_AF(PD13_PWM_INPUT1, 2) | \
PIN_AFIO_AF(PD14_PWM_INPUT2, 2) | \
PIN_AFIO_AF(PD15_MPU_DRDY, 0))

#define VAL_GPIOE_MODER (PIN_MODE_ALTERNATE(PE00_UART8_RX) | \
Expand Down Expand Up @@ -1559,10 +1559,10 @@
#define AF_LINE_UART3_TX 7U
#define AF_PD09_UART3_RX 7U
#define AF_LINE_UART3_RX 7U
#define AF_PD13_SERVO5 2U
#define AF_LINE_SERVO5 2U
#define AF_PD14_SERVO6 2U
#define AF_LINE_SERVO6 2U
#define AF_PD13_PWM_INPUT1 2U
#define AF_LINE_PWM_INPUT1 2U
#define AF_PD14_PWM_INPUT2 2U
#define AF_LINE_PWM_INPUT2 2U
#define AF_PE00_UART8_RX 8U
#define AF_LINE_UART8_RX 8U
#define AF_PE01_UART8_TX 8U
Expand Down Expand Up @@ -1609,14 +1609,14 @@
#define ADC5_ADC 1
#define ADC5_ADC_FN INP
#define ADC5_ADC_INP 8
#define SERVO5_TIM 4
#define SERVO5_TIM_FN CH
#define SERVO5_TIM_CH 2
#define SERVO5_TIM_AF 2
#define SERVO6_TIM 4
#define SERVO6_TIM_FN CH
#define SERVO6_TIM_CH 3
#define SERVO6_TIM_AF 2
#define PWM_INPUT1_TIM 4
#define PWM_INPUT1_TIM_FN CH
#define PWM_INPUT1_TIM_CH 2
#define PWM_INPUT1_TIM_AF 2
#define PWM_INPUT2_TIM 4
#define PWM_INPUT2_TIM_FN CH
#define PWM_INPUT2_TIM_CH 3
#define PWM_INPUT2_TIM_AF 2
#define SERVO4_TIM 1
#define SERVO4_TIM_FN CH
#define SERVO4_TIM_CH 1
Expand Down Expand Up @@ -1658,15 +1658,13 @@
LINE_SPI_SLAVE5, \
LINE_SPI_SLAVE6, \
LINE_SPI_SLAVE7, \
LINE_SERVO5, \
LINE_SERVO6, \
LINE_SPI_SLAVE8, \
LINE_SERVO4, \
LINE_SERVO3, \
LINE_LED1, \
LINE_SERVO2, \
LINE_SERVO1
#define ENERGY_SAVE_INPUTS_SIZE 16
#define ENERGY_SAVE_INPUTS_SIZE 14

#define ENERGY_SAVE_LOWS \
LINE_VDD_5V_PERIPH_EN, \
Expand Down
4 changes: 2 additions & 2 deletions sw/airborne/boards/cube/orange/mcuconf.h
View file
Expand Up @@ -339,7 +339,7 @@
#define STM32_ICU_USE_TIM1 FALSE
#define STM32_ICU_USE_TIM2 FALSE
#define STM32_ICU_USE_TIM3 FALSE
#define STM32_ICU_USE_TIM4 FALSE
#define STM32_ICU_USE_TIM4 TRUE
#define STM32_ICU_USE_TIM5 FALSE
#define STM32_ICU_USE_TIM8 FALSE
#define STM32_ICU_USE_TIM12 FALSE
Expand Down Expand Up @@ -367,7 +367,7 @@
#define STM32_PWM_USE_TIM1 TRUE
#define STM32_PWM_USE_TIM2 FALSE
#define STM32_PWM_USE_TIM3 FALSE
#define STM32_PWM_USE_TIM4 TRUE
#define STM32_PWM_USE_TIM4 FALSE
#define STM32_PWM_USE_TIM5 FALSE
#define STM32_PWM_USE_TIM8 FALSE
#define STM32_PWM_USE_TIM9 FALSE
Expand Down
3 changes: 2 additions & 1 deletion sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
View file
Expand Up @@ -259,7 +259,8 @@ void autopilot_check_in_flight(bool motors_on)
/* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
(fabsf(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL)) {
(fabsf(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL) &&
!motors_on) {
autopilot_in_flight_counter--;
if (autopilot_in_flight_counter == 0) {
autopilot.in_flight = false;
Expand Down
230 changes: 194 additions & 36 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
View file
Expand Up @@ -40,6 +40,13 @@
#include "modules/core/abi.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"

#ifndef GUIDANCE_INDI_QUADPLANE
#define GUIDANCE_INDI_QUADPLANE FALSE
#endif

#if GUIDANCE_INDI_QUADPLANE
#include "firmwares/rotorcraft/stabilization/wls/wls_alloc.h"
#endif

// The acceleration reference is calculated with these gains. If you use GPS,
// they are probably limited by the update rate of your GPS. The default
Expand All @@ -56,9 +63,14 @@
#endif

#ifndef GUIDANCE_INDI_MIN_PITCH
#if GUIDANCE_INDI_QUADPLANE
#define GUIDANCE_INDI_MIN_PITCH -20
#define GUIDANCE_INDI_MAX_PITCH 20
#else
#define GUIDANCE_INDI_MIN_PITCH -120
#define GUIDANCE_INDI_MAX_PITCH 25
#endif
#endif

#ifndef GUIDANCE_INDI_LIFTD_ASQ
#define GUIDANCE_INDI_LIFTD_ASQ 0.20
Expand Down Expand Up @@ -107,6 +119,9 @@ bool take_heading_control = false;

bool force_forward = false;

struct FloatRates ff_rates;
bool ff_rates_set = false;

struct FloatVect3 sp_accel = {0.0,0.0,0.0};
#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
Expand Down Expand Up @@ -144,6 +159,7 @@ Butterworth2LowPass roll_filt;
Butterworth2LowPass pitch_filt;
Butterworth2LowPass thrust_filt;
Butterworth2LowPass accely_filt;
Butterworth2LowPass accel_bodyz_filt;

struct FloatVect2 desired_airspeed;

Expand All @@ -152,6 +168,31 @@ struct FloatMat33 Ga_inv;
struct FloatVect3 euler_cmd;

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF;
float bodyz_filter_cutoff = 0.2;

#if GUIDANCE_INDI_QUADPLANE
float Gmat_quadplane[3][4];
float *Bwls_quadplane[3];

int num_iter_quadplane= 0;
float guidance_indi_du[4];
float Wv_quadplane[3] = {10., 10., 10.};
float Wu_quadplane[4] = {10., 10., 100., 1.};

#ifndef GUIDANCE_INDI_THRUST_Z_EFF
#error "You need to define GUIDANCE_INDI_THRUST_Z_EFF"
#else
float guidance_indi_thrust_z_eff = GUIDANCE_INDI_THRUST_Z_EFF;
#endif

#ifndef GUIDANCE_INDI_THRUST_X_EFF
#error "You need to define GUIDANCE_INDI_THRUST_X_EFF"
#else
float guidance_indi_thrust_x_eff = GUIDANCE_INDI_THRUST_X_EFF;
#endif

float guidance_indi_pitch_pref_deg = 0;
#endif

float guidance_indi_hybrid_heading_sp = 0.f;
struct FloatEulers guidance_euler_cmd;
Expand All @@ -167,9 +208,23 @@ static void vel_sp_cb(uint8_t sender_id, struct FloatVect3 *vel_sp);
struct FloatVect3 indi_vel_sp = {0.0, 0.0, 0.0};
float time_of_vel_sp = 0.0;

#ifndef GUIDANCE_INDI_LIFT_D_ID
#define GUIDANCE_INDI_LIFT_D_ID ABI_BROADCAST
#endif
abi_event lift_d_ev;
static void lift_d_cb(uint8_t sender_id, float lift_d);
float indi_lift_d_abi = 0;
float time_of_lift_d = 0.0;

void guidance_indi_propagate_filters(void);
static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
static float guidance_indi_get_liftd(float pitch, float theta);
#if GUIDANCE_INDI_QUADPLANE
// For Quadplane types
static void guidance_indi_calcg_wls(struct FloatVect3 a_diff);
static float guidance_indi_get_liftd_abi(void);
#else
static void guidance_indi_calcg_wing(struct FloatMat33 *Gmat);
static float guidance_indi_get_liftd(float pitch, float theta);
#endif

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
Expand Down Expand Up @@ -198,6 +253,7 @@ void guidance_indi_init(void)
{
/*AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);*/
AbiBindMsgVEL_SP(GUIDANCE_INDI_VEL_SP_ID, &vel_sp_ev, vel_sp_cb);
AbiBindMsgLIFT_D(GUIDANCE_INDI_LIFT_D_ID, &lift_d_ev, lift_d_cb);

float tau = 1.0/(2.0*M_PI*filter_cutoff);
float sample_time = 1.0/PERIODIC_FREQUENCY;
Expand All @@ -212,6 +268,12 @@ void guidance_indi_init(void)
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GUIDANCE_INDI_HYBRID, send_guidance_indi_hybrid);
#endif

#if GUIDANCE_INDI_QUADPLANE
for (int8_t i = 0; i < 3; i++) {
Bwls_quadplane[i] = Gmat_quadplane[i];
}
#endif
}

/**
Expand All @@ -228,6 +290,7 @@ void guidance_indi_enter(void) {
guidance_indi_hybrid_heading_sp = stateGetNedToBodyEulers_f()->psi;

float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
float tau_bodyz = 1.0/(2.0*M_PI*bodyz_filter_cutoff);
float sample_time = 1.0 / PERIODIC_FREQUENCY;
for (int8_t i = 0; i < 3; i++) {
init_butterworth_2_low_pass(&filt_accel_ned[i], tau, sample_time, 0.0);
Expand All @@ -240,6 +303,7 @@ void guidance_indi_enter(void) {
init_butterworth_2_low_pass(&pitch_filt, tau, sample_time, eulers_zxy.theta);
init_butterworth_2_low_pass(&thrust_filt, tau, sample_time, thrust_in);
init_butterworth_2_low_pass(&accely_filt, tau, sample_time, 0.0);
init_butterworth_2_low_pass(&accel_bodyz_filt, tau_bodyz, sample_time, -9.81);
}

#include "firmwares/rotorcraft/navigation.h"
Expand Down Expand Up @@ -281,11 +345,6 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
sp_accel.z = -(radio_control.values[RADIO_THROTTLE]-4500)*8.0/9600.0;
#endif

// Calculate matrix of partial derivatives
guidance_indi_calcg_wing(&Ga);
// Invert this matrix
MAT33_INV(Ga_inv, Ga);

struct FloatVect3 accel_filt;
accel_filt.x = filt_accel_ned[0].o[0];
accel_filt.y = filt_accel_ned[1].o[0];
Expand All @@ -309,10 +368,30 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
#endif
#endif

//Calculate roll,pitch and thrust command
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);

AbiSendMsgTHRUST(THRUST_INCREMENT_ID, euler_cmd.z);
#if GUIDANCE_INDI_QUADPLANE
// Perform WLS for quadplane types
guidance_indi_calcg_wls(a_diff);
euler_cmd.x = guidance_indi_du[0];
euler_cmd.y = guidance_indi_du[1];
euler_cmd.z = guidance_indi_du[2];
#else
// Calculate matrix of partial derivatives
guidance_indi_calcg_wing(&Ga);
// Invert this matrix
MAT33_INV(Ga_inv, Ga);

//Calculate roll,pitch and thrust command
MAT33_VECT3_MUL(euler_cmd, Ga_inv, a_diff);
#endif
struct FloatVect3 thrust_vect;
#if GUIDANCE_INDI_QUADPLANE
thrust_vect.x = guidance_indi_du[3];
#else
thrust_vect.x = 0.0;
#endif
thrust_vect.y = 0.0;
thrust_vect.z = euler_cmd.z;
AbiSendMsgTHRUST(THRUST_INCREMENT_ID, thrust_vect);

// Coordinated turn
// feedforward estimate angular rotation omega = g*tan(phi)/v
Expand Down Expand Up @@ -630,48 +709,105 @@ void guidance_indi_propagate_filters(void) {
// Propagate filter for sideslip correction
float accely = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->y);
update_butterworth_2_low_pass(&accely_filt, accely);

// Propagate filter for thrust/lift estimate
float accelz = ACCEL_FLOAT_OF_BFP(stateGetAccelBody_i()->z);
update_butterworth_2_low_pass(&accel_bodyz_filt, accelz);
}

/**
* Calculate the matrix of partial derivatives of the roll, pitch and thrust
* w.r.t. the NED accelerations, taking into account the lift of a wing that is
* horizontal at -90 degrees pitch
* Perform WLS
*
* @param Gmat array to write the matrix to [3x3]
* @param a_diff 3D vector to write the acceration error
*/
void guidance_indi_calcg_wing(struct FloatMat33 *Gmat) {

void guidance_indi_calcg_wls(struct FloatVect3 a_diff) {
/*Pre-calculate sines and cosines*/
float sphi = sinf(eulers_zxy.phi);
float cphi = cosf(eulers_zxy.phi);
float stheta = sinf(eulers_zxy.theta);
float ctheta = cosf(eulers_zxy.theta);
float spsi = sinf(eulers_zxy.psi);
float cpsi = cosf(eulers_zxy.psi);
//minus gravity is a guesstimate of the thrust force, thrust measurement would be better

float pitch_pref_rad = RadOfDeg(guidance_indi_pitch_pref_deg);

#ifndef GUIDANCE_INDI_PITCH_EFF_SCALING
#define GUIDANCE_INDI_PITCH_EFF_SCALING 1.0
#endif

/*Amount of lift produced by the wing*/
float pitch_lift = eulers_zxy.theta;
Bound(pitch_lift,-M_PI_2,0);
float lift = sinf(pitch_lift)*9.81;
float T = cosf(pitch_lift)*-9.81;
float lift_thrust_bz = accel_bodyz_filt.o[0]; // Sum of lift and thrust in boxy z axis (level flight)

// get the derivative of the lift wrt to theta
float liftd = guidance_indi_get_liftd(stateGetAirspeed_f(), eulers_zxy.theta);

RMAT_ELMT(*Gmat, 0, 0) = cphi*ctheta*spsi*T + cphi*spsi*lift;
RMAT_ELMT(*Gmat, 1, 0) = -cphi*ctheta*cpsi*T - cphi*cpsi*lift;
RMAT_ELMT(*Gmat, 2, 0) = -sphi*ctheta*T -sphi*lift;
RMAT_ELMT(*Gmat, 0, 1) = (ctheta*cpsi - sphi*stheta*spsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING + sphi*spsi*liftd;
RMAT_ELMT(*Gmat, 1, 1) = (ctheta*spsi + sphi*stheta*cpsi)*T*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*cpsi*liftd;
RMAT_ELMT(*Gmat, 2, 1) = -cphi*stheta*T*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;
RMAT_ELMT(*Gmat, 0, 2) = stheta*cpsi + sphi*ctheta*spsi;
RMAT_ELMT(*Gmat, 1, 2) = stheta*spsi - sphi*ctheta*cpsi;
RMAT_ELMT(*Gmat, 2, 2) = cphi*ctheta;
float liftd = guidance_indi_get_liftd_abi();

// Calc assumed body acceleration setpoint and error
float accel_bx_sp = cpsi * sp_accel.x + spsi * sp_accel.y;
float accel_bx = cpsi * filt_accel_ned[0].o[0] + spsi * filt_accel_ned[1].o[0];
float accel_bx_err = accel_bx_sp - accel_bx;

Gmat_quadplane[0][0] = -sphi*stheta*lift_thrust_bz;
Gmat_quadplane[1][0] = -cphi*lift_thrust_bz;
Gmat_quadplane[2][0] = -sphi*ctheta*lift_thrust_bz;

Gmat_quadplane[0][1] = cphi*ctheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING;
Gmat_quadplane[1][1] = sphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING - sphi*liftd;
Gmat_quadplane[2][1] = -cphi*stheta*lift_thrust_bz*GUIDANCE_INDI_PITCH_EFF_SCALING + cphi*liftd;

Gmat_quadplane[0][2] = cphi*stheta;
Gmat_quadplane[1][2] = -sphi;
Gmat_quadplane[2][2] = cphi*ctheta;

Gmat_quadplane[0][3] = ctheta;
Gmat_quadplane[1][3] = 0;
Gmat_quadplane[2][3] = -stheta;

// Perform WLS
// WLS Control Allocator

// Convert acceleration error to body axis system
float body_v[3];
body_v[0] = cpsi * a_diff.x + spsi * a_diff.y;
body_v[1] = -spsi * a_diff.x + cpsi * a_diff.y;
body_v[2] = a_diff.z;

// Thrust z limits
float du_min_thrust_z = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST]) * guidance_indi_thrust_z_eff;
float du_max_thrust_z = -stabilization_cmd[COMMAND_THRUST] * guidance_indi_thrust_z_eff;

Bound(du_min_thrust_z, -50., 0.);
Bound(du_max_thrust_z, 0., 50.);

float roll_limit_rad = GUIDANCE_H_MAX_BANK;
float max_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MAX_PITCH);
float min_pitch_limit_rad = RadOfDeg(GUIDANCE_INDI_MIN_PITCH);

// Set lower limits
float du_min[4];
du_min[0] = -roll_limit_rad - roll_filt.o[0]; //roll
du_min[1] = min_pitch_limit_rad - pitch_filt.o[0]; // pitch
du_min[2] = du_min_thrust_z;
du_min[3] = -stabilization_cmd[COMMAND_THRUST_X]*guidance_indi_thrust_x_eff;

// Set upper limits limits
float du_max[4];
du_max[0] = roll_limit_rad - roll_filt.o[0]; //roll
du_max[1] = max_pitch_limit_rad - pitch_filt.o[0]; // pitch
du_max[2] = du_max_thrust_z;
du_max[3] = (MAX_PPRZ - stabilization_cmd[COMMAND_THRUST_X])*guidance_indi_thrust_x_eff;

// Set prefered states
float du_pref[4];
du_pref[0] = 0; // prefered delta roll angle
du_pref[1] = -pitch_filt.o[0] + pitch_pref_rad;// prefered delta pitch angle
du_pref[2] = du_max[2]; // Low thrust better for efficiency
du_pref[3] = accel_bx_err - 9.81 * sinf(pitch_filt.o[0]);

int16_t n_u = CA_N_U_OUTER;
int16_t n_v = CA_N_V_OUTER;

num_iter_quadplane =
wls_alloc(guidance_indi_du, body_v, du_min, du_max, Bwls_quadplane, 0, 0, Wv_quadplane, Wu_quadplane, du_pref, 100000, 10, n_u, n_v);
}

/**
Expand Down Expand Up @@ -710,6 +846,20 @@ float guidance_indi_get_liftd(float airspeed, float theta) {
return liftd;
}

/**
* @brief Get the derivative of lift w.r.t. pitch from a ABI message
*
* @return The derivative of lift w.r.t. pitch
*/
float guidance_indi_get_liftd_abi(void) {
float liftd = 0.0;
float dt = get_sys_time_float() - time_of_lift_d;
if (dt < 0.5) {
liftd = indi_lift_d_abi;
}
return liftd;
}

/**
* ABI callback that obtains the velocity setpoint from a module
*/
Expand All @@ -721,6 +871,14 @@ static void vel_sp_cb(uint8_t sender_id __attribute__((unused)), struct FloatVec
time_of_vel_sp = get_sys_time_float();
}

/**
* ABI callback that obtains the liftd from a module
*/
static void lift_d_cb(uint8_t sender_id __attribute__((unused)), float lift_d)
{
indi_lift_d_abi = lift_d;
time_of_lift_d = get_sys_time_float();
}

#if GUIDANCE_INDI_HYBRID_USE_AS_DEFAULT
// guidance indi control function is implementing the default functions of guidance
Expand Down Expand Up @@ -757,21 +915,21 @@ int32_t guidance_v_run_pos(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_HYBRID_V_POS;
return (int32_t)thrust_in; // nothing to do
return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
}

int32_t guidance_v_run_speed(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_HYBRID_V_SPEED;
return (int32_t)thrust_in; // nothing to do
return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
}

int32_t guidance_v_run_accel(bool in_flight UNUSED, struct VerticalGuidance *gv)
{
_gv = gv;
_v_mode = GUIDANCE_INDI_HYBRID_V_ACCEL;
return (int32_t)thrust_in; // nothing to do
return (int32_t)stabilization_cmd[COMMAND_THRUST]; // nothing to do
}

#endif
Expand Down
8 changes: 8 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.h
View file
Expand Up @@ -71,6 +71,14 @@ struct guidance_indi_hybrid_params {
extern struct FloatVect3 sp_accel;
extern struct FloatVect3 gi_speed_sp;

extern struct FloatRates ff_rates;
extern bool ff_rates_set;

extern float guidance_indi_pitch_pref_deg;

extern float Wu_quadplane[4];
extern float Wv_quadplane[3];

extern struct guidance_indi_hybrid_params gih_params;
extern float guidance_indi_specific_force_gain;
extern float guidance_indi_max_airspeed;
Expand Down
907 changes: 907 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_rot_wing.c
View file

Large diffs are not rendered by default.

106 changes: 106 additions & 0 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_rot_wing.h
View file
@@ -0,0 +1,106 @@
/*
* Copyright (C) 2015 Ewoud Smeur <ewoud.smeur@gmail.com>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/

/**
* @file firmwares/rotorcraft/guidance/guidance_indi_rot_wing.h
*
* A guidance mode based on Incremental Nonlinear Dynamic Inversion
* Come to ICRA2016 to learn more!
*
*/

#ifndef GUIDANCE_INDI_ROT_WING_H
#define GUIDANCE_INDI_ROT_WING_H

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "filters/high_pass_filter.h"
#include "firmwares/rotorcraft/guidance.h"
#include "firmwares/rotorcraft/stabilization.h"

// TODO change names for _indi_hybrid_

extern void guidance_indi_init(void);
extern void guidance_indi_enter(void);

enum GuidanceIndiHybrid_HMode {
GUIDANCE_INDI_HYBRID_H_POS,
GUIDANCE_INDI_HYBRID_H_SPEED,
GUIDANCE_INDI_HYBRID_H_ACCEL
};

enum GuidanceIndiHybrid_VMode {
GUIDANCE_INDI_HYBRID_V_POS,
GUIDANCE_INDI_HYBRID_V_SPEED,
GUIDANCE_INDI_HYBRID_V_ACCEL
};

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_propagate_filters(void);

struct guidance_indi_hybrid_params {
float pos_gain;
float pos_gainz;
float speed_gain;
float speed_gainz;
float heading_bank_gain;
};

extern struct guidance_indi_hybrid_params gih_params;
extern float guidance_indi_specific_force_gain;
extern float guidance_indi_max_airspeed;
extern float nav_max_speed;
extern bool take_heading_control;
extern bool force_forward; ///< forward flight for hybrid nav

extern struct FloatRates ff_rates;
extern bool ff_rates_set;

extern float guidance_indi_max_bank;
extern float push_first_order_constant;
extern float hover_motor_slowdown;
extern float a_diff_limit;
extern float a_diff_limit_z;
extern float rot_wing_max_pitch_limit_deg;
extern float rot_wing_min_pitch_limit_deg;
extern float pitch_pref_deg;
extern float airspeed_turn_lower_bound;

extern bool hover_motors_active;
extern bool bool_disable_hover_motors;

extern bool weather_vane_on;
extern float weather_vane_gain;

extern float pitch_priority_factor;
extern float roll_priority_factor;
extern float thrust_priority_factor;
extern float pusher_priority_factor;

extern float horizontal_accel_weight;
extern float vertical_accel_weight;

extern float climb_vspeed_fwd;
extern float descend_vspeed_fwd;

#endif /* GUIDANCE_INDI_ROT_WING_H */
132 changes: 112 additions & 20 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
View file
Expand Up @@ -45,6 +45,9 @@
#include "wls/wls_alloc.h"
#include <stdio.h>

// For feed forward rates
#include "firmwares/rotorcraft/guidance/guidance_indi_hybrid.h"

// Factor that the estimated G matrix is allowed to deviate from initial one
#define INDI_ALLOWED_G_FACTOR 2.0

Expand Down Expand Up @@ -72,6 +75,12 @@
#define INDI_HROTTLE_LIMIT_AIRSPEED_FWD 8.0
#endif

#if INDI_OUTPUTS > 4
#ifndef STABILIZATION_INDI_G1_THRUST_X
#error "You must define STABILIZATION_INDI_G1_THRUST_X for your number of INDI_OUTPUTS"
#endif
#endif

float du_min[INDI_NUM_ACT];
float du_max[INDI_NUM_ACT];
float du_pref[INDI_NUM_ACT];
Expand All @@ -83,7 +92,9 @@ static void lms_estimation(void);
static void get_actuator_state(void);
static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra);
static void calc_g2_element(float dx_error, int8_t j, float mu_extra);
#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
static void calc_g1g2_pseudo_inv(void);
#endif
static void bound_g_mat(void);

int32_t stabilization_att_indi_cmd[COMMANDS_NB];
Expand Down Expand Up @@ -116,6 +127,14 @@ bool act_is_servo[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_SERVO;
bool act_is_servo[INDI_NUM_ACT] = {0};
#endif

#ifdef STABILIZATION_INDI_ACT_IS_THRUSTER_X
bool act_is_thruster_x[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_IS_THRUSTER_X;
#else
bool act_is_thruster_x[INDI_NUM_ACT] = {0};
#endif

bool act_is_thruster_z[INDI_NUM_ACT];

#ifdef STABILIZATION_INDI_ACT_PREF
// Preferred (neutral, least energy) actuator value
float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
Expand All @@ -130,7 +149,11 @@ float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
#else
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
#if INDI_OUTPUTS == 5
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100, 100};
#else
static float Wv[INDI_OUTPUTS] = {1000, 1000, 1, 100};
#endif
#endif

/**
Expand Down Expand Up @@ -175,6 +198,7 @@ float act_obs[INDI_NUM_ACT];

// Number of actuators used to provide thrust
int32_t num_thrusters;
int32_t num_thrusters_x;

struct Int32Eulers stab_att_sp_euler;
struct Int32Quat stab_att_sp_quat;
Expand All @@ -183,15 +207,23 @@ abi_event rpm_ev;
static void rpm_cb(uint8_t sender_id, struct rpm_act_t *rpm_msg, uint8_t num_act);

abi_event thrust_ev;
static void thrust_cb(uint8_t sender_id, float thrust_increment);
float indi_thrust_increment;
static void thrust_cb(uint8_t sender_id, struct FloatVect3 thrust_increment);
struct FloatVect3 indi_thrust_increment;
bool indi_thrust_increment_set = false;

float g1g2_pseudo_inv[INDI_NUM_ACT][INDI_OUTPUTS];
float g2[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
#if INDI_OUTPUTS == 5
float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW,
STABILIZATION_INDI_G1_THRUST, STABILIZATION_INDI_G1_THRUST_X
};
#else
float g1[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
};
#endif

float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
float g1_est[INDI_OUTPUTS][INDI_NUM_ACT];
float g2_est[INDI_NUM_ACT];
Expand All @@ -204,6 +236,7 @@ Butterworth2LowPass measurement_lowpass_filters[3];
Butterworth2LowPass estimation_output_lowpass_filters[3];
Butterworth2LowPass acceleration_lowpass_filter;
static struct FirstOrderLowPass rates_filt_fo[3];
Butterworth2LowPass rates_filt_so[3];

struct FloatVect3 body_accel_f;

Expand All @@ -214,10 +247,10 @@ void sum_g1_g2(void);
#include "modules/datalink/telemetry.h"
static void send_indi_g(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1_est[0],
INDI_NUM_ACT, g1_est[1],
INDI_NUM_ACT, g1_est[2],
INDI_NUM_ACT, g1_est[3],
pprz_msg_send_INDI_G(trans, dev, AC_ID, INDI_NUM_ACT, g1g2[0],
INDI_NUM_ACT, g1g2[1],
INDI_NUM_ACT, g1g2[2],
INDI_NUM_ACT, g1g2[3],
INDI_NUM_ACT, g2_est);
}

Expand Down Expand Up @@ -283,7 +316,10 @@ void stabilization_indi_init(void)

//Calculate G1G2_PSEUDO_INVERSE
sum_g1_g2();

#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
calc_g1g2_pseudo_inv();
#endif

int8_t i;
// Initialize the array of pointers to the rows of g1g2
Expand All @@ -300,8 +336,14 @@ void stabilization_indi_init(void)

// Assume all non-servos are delivering thrust
num_thrusters = INDI_NUM_ACT;
num_thrusters_x = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
num_thrusters -= act_is_servo[i];
num_thrusters -= act_is_thruster_x[i];

num_thrusters_x += act_is_thruster_x[i];

act_is_thruster_z[i] = !act_is_servo[i] && !act_is_thruster_x[i];
}

#if PERIODIC_TELEMETRY
Expand Down Expand Up @@ -353,12 +395,21 @@ void init_filters(void)
// Filtering of the accel body z
init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);

#if INDI_FILTER_RATES_SECOND_ORDER
tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P);
init_butterworth_2_low_pass(&rates_filt_so[0], tau, sample_time, 0.0);
tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q);
init_butterworth_2_low_pass(&rates_filt_so[1], tau, sample_time, 0.0);
tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R);
init_butterworth_2_low_pass(&rates_filt_so[2], tau, sample_time, 0.0);
#else
// Init rate filter for feedback
float time_constants[3] = {1.0 / (2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P), 1.0 / (2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q), 1.0 / (2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R)};

init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
#endif
}

/**
Expand Down Expand Up @@ -463,17 +514,29 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
//Note that due to the delay, the PD controller may need relaxed gains.
struct FloatRates rates_filt;
#if STABILIZATION_INDI_FILTER_ROLL_RATE
#if INDI_FILTER_RATES_SECOND_ORDER
rates_filt.p = update_butterworth_2_low_pass(&rates_filt_so[0], body_rates->p);
#else
rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
#endif
#else
rates_filt.p = body_rates->p;
#endif
#if STABILIZATION_INDI_FILTER_PITCH_RATE
#if INDI_FILTER_RATES_SECOND_ORDER
rates_filt.p = update_butterworth_2_low_pass(&rates_filt_so[1], body_rates->q);
#else
rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
#endif
#else
rates_filt.q = body_rates->q;
#endif
#if STABILIZATION_INDI_FILTER_YAW_RATE
#if INDI_FILTER_RATES_SECOND_ORDER
rates_filt.p = update_butterworth_2_low_pass(&rates_filt_so[2], body_rates->r);
#else
rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
#endif
#else
rates_filt.r = body_rates->r;
#endif
Expand All @@ -495,30 +558,48 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
use_increment = 1.0;
}

float v_thrust = 0.0;
struct FloatVect3 v_thrust;
v_thrust.x = 0.0;
v_thrust.y = 0.0;
v_thrust.z = 0.0;
if (indi_thrust_increment_set) {
v_thrust = indi_thrust_increment;

//update thrust command such that the current is correctly estimated
stabilization_cmd[COMMAND_THRUST] = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * -((int32_t) act_is_servo[i] - 1);
stabilization_cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_is_thruster_z[i];
}
stabilization_cmd[COMMAND_THRUST] /= num_thrusters;

#if INDI_OUTPUTS == 5
stabilization_cmd[COMMAND_THRUST_X] = 0;
for (i = 0; i < INDI_NUM_ACT; i++) {
stabilization_cmd[COMMAND_THRUST_X] += actuator_state[i] * (int32_t) act_is_thruster_x[i];
}
stabilization_cmd[COMMAND_THRUST_X] /= num_thrusters_x;
#endif

} else {
// incremental thrust
for (i = 0; i < INDI_NUM_ACT; i++) {
v_thrust +=
v_thrust.z +=
(stabilization_cmd[COMMAND_THRUST] - use_increment*actuator_state_filt_vect[i]) * Bwls[3][i];
#if INDI_OUTPUTS == 5
v_thrust.x +=
(stabilization_cmd[COMMAND_THRUST_X] - use_increment*actuator_state_filt_vect[i]) * Bwls[4][i];
#endif
}
}

// The control objective in array format
indi_v[0] = (angular_accel_ref.p - use_increment*angular_acceleration[0]);
indi_v[1] = (angular_accel_ref.q - use_increment*angular_acceleration[1]);
indi_v[2] = (angular_accel_ref.r - use_increment*angular_acceleration[2] + g2_times_du);
indi_v[3] = v_thrust;
indi_v[3] = v_thrust.z;
#if INDI_OUTPUTS == 5
indi_v[4] = v_thrust.x;
#endif

#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
// Calculate the increment for each actuator
Expand All @@ -529,11 +610,11 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
+ (g1g2_pseudo_inv[i][3] * indi_v[3]);
}
#else
// Calculate the min and max increments
for (i = 0; i < INDI_NUM_ACT; i++) {
du_min[i] = -MAX_PPRZ * act_is_servo[i] - use_increment*actuator_state_filt_vect[i];
du_max[i] = MAX_PPRZ - use_increment*actuator_state_filt_vect[i];
du_pref[i] = act_pref[i] - use_increment*actuator_state_filt_vect[i];
// Calculate the min and max increments
for (i = 0; i < INDI_NUM_ACT; i++) {
du_min[i] = -MAX_PPRZ * act_is_servo[i] - use_increment*actuator_state_filt_vect[i];
du_max[i] = MAX_PPRZ - use_increment*actuator_state_filt_vect[i];
du_pref[i] = act_pref[i] - use_increment*actuator_state_filt_vect[i];

#ifdef GUIDANCE_INDI_MIN_THROTTLE
float airspeed = stateGetAirspeed_f();
Expand All @@ -550,9 +631,12 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
#endif
}

// WLS Control Allocator
num_iter =
wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, indi_Wu, du_pref, 10000, 10);
int16_t n_u = CA_N_U_INNER;
int16_t n_v = CA_N_V_INNER;

// WLS Control Allocator
num_iter =
wls_alloc(indi_du, indi_v, du_min, du_max, Bwls, 0, 0, Wv, indi_Wu, du_pref, 10000, 10, n_u, n_v);
#endif

if (in_flight) {
Expand Down Expand Up @@ -646,6 +730,11 @@ void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
rate_sp.q = indi_gains.att.q * att_fb.y / indi_gains.rate.q;
rate_sp.r = indi_gains.att.r * att_fb.z / indi_gains.rate.r;

if (ff_rates_set) {
RATES_ADD(rate_sp, ff_rates);
ff_rates_set = false;
}

// Store for telemetry
angular_rate_ref.p = rate_sp.p;
angular_rate_ref.q = rate_sp.q;
Expand Down Expand Up @@ -837,6 +926,7 @@ void sum_g1_g2(void) {
}
}

#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
/**
* Function that calculates the pseudo-inverse of (G1+G2).
* Make sure to sum of G1 and G2 before running this!
Expand Down Expand Up @@ -880,10 +970,12 @@ void calc_g1g2_pseudo_inv(void)
}
}
}
#endif

static void rpm_cb(uint8_t sender_id UNUSED, struct rpm_act_t *rpm_msg UNUSED, uint8_t num_act UNUSED)
{
#if INDI_RPM_FEEDBACK
#warning INDI_RPM_FEEDBACK
int8_t i;
for (i = 0; i < num_act; i++) {
// Sanity check that index is valid
Expand All @@ -900,7 +992,7 @@ static void rpm_cb(uint8_t sender_id UNUSED, struct rpm_act_t *rpm_msg UNUSED, u
/**
* ABI callback that obtains the thrust increment from guidance INDI
*/
static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
static void thrust_cb(uint8_t UNUSED sender_id, struct FloatVect3 thrust_increment)
{
indi_thrust_increment = thrust_increment;
indi_thrust_increment_set = true;
Expand Down
6 changes: 6 additions & 0 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h
View file
Expand Up @@ -34,11 +34,17 @@ extern struct Int32Quat stab_att_sp_quat; ///< with #INT32_QUAT_FRAC
extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
extern float g1g2[INDI_OUTPUTS][INDI_NUM_ACT];
extern float actuator_state_filt_vect[INDI_NUM_ACT];
extern bool act_is_servo[INDI_NUM_ACT];

extern bool indi_use_adaptive;

extern float *Bwls[INDI_OUTPUTS];

extern float thrust_bx_eff;
extern float thrust_bx_act_dyn;
extern float actuator_thrust_bx_pprz;
extern float thrust_bx_state_filt;

extern float act_pref[INDI_NUM_ACT];

extern float indi_Wu[INDI_NUM_ACT];
Expand Down
35 changes: 27 additions & 8 deletions sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.c
View file
Expand Up @@ -52,12 +52,33 @@ void print_in_and_outputs(int n_c, int n_free, float** A_free_ptr, float* d, flo
#define WLS_VERBOSE FALSE

// Problem size needs to be predefined to avoid having to use VLAs
#ifndef CA_N_V
#error CA_N_V needs to be defined!
#ifdef CA_N_V_INNER
#ifdef CA_N_V_OUTER

#if CA_N_V_INNER > CA_N_V_OUTER
#define CA_N_V CA_N_V_INNER
#else
#define CA_N_V CA_N_V_OUTER
#endif

#if CA_N_U_INNER > CA_N_U_OUTER
#define CA_N_U CA_N_U_INNER
#else
#define CA_N_U CA_N_U_OUTER
#endif

#ifndef CA_N_U
#error CA_N_U needs to be defined!
#else
#define CA_N_V CA_N_V_INNER
#define CA_N_U CA_N_U_INNER
#endif

#elif CA_N_U_OUTER
#define CA_N_V CA_N_V_OUTER
#define CA_N_U CA_N_U_OUTER
#else

#error CA_N_V_INNER or CA_N_V_OUTER needs to be defined!

#endif

#define CA_N_C (CA_N_U+CA_N_V)
Expand Down Expand Up @@ -111,14 +132,12 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x) {
*/
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
float* u_guess, float* W_init, float* Wv, float* Wu, float* up,
float gamma_sq, int imax) {
float gamma_sq, int imax, int n_u, int n_v) {
// allocate variables, use defaults where parameters are set to 0
if(!gamma_sq) gamma_sq = 100000;
if(!imax) imax = 100;

int n_c = CA_N_C;
int n_u = CA_N_U;
int n_v = CA_N_V;
int n_c = n_u + n_v;

float A[CA_N_C][CA_N_U];
float A_free[CA_N_C][CA_N_U];
Expand Down
2 changes: 1 addition & 1 deletion sw/airborne/firmwares/rotorcraft/stabilization/wls/wls_alloc.h
View file
Expand Up @@ -58,4 +58,4 @@ void qr_solve_wrapper(int m, int n, float** A, float* b, float* x);
*/
int wls_alloc(float* u, float* v, float* umin, float* umax, float** B,
float* u_guess, float* W_init, float* Wv, float* Wu,
float* ud, float gamma, int imax);
float* ud, float gamma, int imax, int n_u, int n_v);
8 changes: 8 additions & 0 deletions sw/airborne/modules/core/abi_sender_ids.h
View file
Expand Up @@ -624,4 +624,12 @@
#define VEL_SP_FCR_ID 1 // Approach Moving Target
#endif

/*
* IDs of LIFT_D senders
*/

#ifndef LIFT_D_SCHED_ID
#define LIFT_D_SCHED_ID 1
#endif

#endif /* ABI_SENDER_IDS_H */
461 changes: 461 additions & 0 deletions sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing_v3b.c
View file

Large diffs are not rendered by default.

59 changes: 59 additions & 0 deletions sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing_v3b.h
View file
@@ -0,0 +1,59 @@
/*
* Copyright (C) 2022 D.C. van Wijngaarden <D.C.vanWijngaarden@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/ctrl/ctrl_eff_sched_rot_wing_v3b.h"
* @author D.C. van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Crtl effectiveness scheduler for thr rotating wing drone V3
*/

#ifndef CTRL_EFF_SCHED_ROT_WING_V3B_H
#define CTRL_EFF_SCHED_ROT_WING_V3B_H

#include "std.h"

// Define settings
extern float lift_d_multiplier;
extern float g1_p_multiplier;
extern float g1_q_multiplier;
extern float g1_r_multiplier;
extern float g1_t_multiplier;

extern float pitch_angle_set;
extern float pitch_angle_range;

extern float rot_wing_aerodynamic_eff_const_g1_p[1];
extern float rot_wing_aerodynamic_eff_const_g1_q[1];
extern float rot_wing_aerodynamic_eff_const_g1_r[1];

extern bool wing_rotation_sched_activated;
extern bool pusher_sched_activated;

extern float pitch_priority_factor;
extern float roll_priority_factor;
extern float thrust_priority_factor;
extern float pusher_priority_factor;

extern bool hover_motors_active;
extern bool bool_disable_hover_motors;

extern void init_eff_scheduling(void);
extern void event_eff_scheduling(void);

#endif // CTRL_EFF_SCHED_ROT_WING_V3_H
2 changes: 1 addition & 1 deletion sw/airborne/modules/datalink/mavlink.c
View file
Expand Up @@ -500,7 +500,7 @@ static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_devic
}
mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
mav_type,
MAV_AUTOPILOT_PPZ,
MAV_AUTOPILOT_ARDUPILOTMEGA,
mav_mode,
0, // custom_mode
mav_state);
Expand Down
1,559 changes: 1,559 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.cpp
View file

Large diffs are not rendered by default.

145 changes: 145 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw.h
View file
@@ -0,0 +1,145 @@
#ifndef EKF_AW_H
#define EKF_AW_H

#ifdef __cplusplus
extern "C" {
#endif

#include "std.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"

// Settings
struct ekfAwParameters {
// Q
float Q_accel; ///< accel process noise
float Q_gyro; ///< gyro process noise
float Q_mu; ///< wind process noise
float Q_k; ///< offset process noise

// R
float R_V_gnd; ///< speed measurement noise
float R_accel_filt[3]; ///< filtered accel measurement noise
float R_V_pitot; ///< airspeed measurement noise

// Model Based Parameters
float vehicle_mass;

// X Axis
float k_fx_drag[2]; // Temporary setting for fuselage + hover prop
float k_fx_fuselage[4];
float k_fx_hover[3];
float k_fx_wing[5];
float k_fx_push[3];
float k_fx_elev[3];

// Y Axis
float k_fy_beta;
float k_fy_v;
float k_fy_wing[5];

// Z Axis
float k_fz_fuselage[4];
float k_fz_wing[4];
float k_fz_hover[5];
float k_fz_elev[2];

// Other options
bool use_model[3];
bool use_pitot;
bool propagate_offset;
bool quick_convergence;
};

struct ekfHealth{
bool healthy;
uint16_t crashes_n;
};

extern struct ekfAwParameters ekf_aw_params;

// Init functions
extern void ekf_aw_init(void);
extern void ekf_aw_reset(void);

// Filtering functions
extern void ekf_aw_propagate(struct FloatVect3 *acc,struct FloatRates *gyro, struct FloatEulers *euler, float *pusher_RPM,float *hover_RPM_array, float *skew, float *elevator_angle, struct FloatVect3 * V_gnd, struct FloatVect3 *acc_filt, float *V_pitot,float dt);

// Getter/Setter functions
extern struct NedCoor_f ekf_aw_get_speed_body(void);
extern struct NedCoor_f ekf_aw_get_wind_ned(void);
extern struct NedCoor_f ekf_aw_get_offset(void);
extern struct FloatVect3 ekf_aw_get_innov_V_gnd(void);
extern struct FloatVect3 ekf_aw_get_innov_accel_filt(void);
extern float ekf_aw_get_innov_V_pitot(void);
extern void ekf_aw_get_meas_cov(float meas_cov[7]);
extern void ekf_aw_get_state_cov(float state_cov[9]);
extern void ekf_aw_get_process_cov(float process_cov[9]);
extern void ekf_aw_get_fuselage_force(float force[3]);
extern void ekf_aw_get_wing_force(float force[3]);
extern void ekf_aw_get_elevator_force(float force[3]);
extern void ekf_aw_get_hover_force(float force[3]);
extern void ekf_aw_get_pusher_force(float force[3]);
extern struct ekfAwParameters *ekf_aw_get_param_handle(void);

extern void ekf_aw_set_speed_body(struct NedCoor_f *s);
extern void ekf_aw_set_wind(struct NedCoor_f *s);
extern void ekf_aw_set_offset(struct NedCoor_f *s);
extern struct ekfHealth ekf_aw_get_health(void);

// Settings handlers
extern void ekf_aw_update_params(void);
extern void ekf_aw_reset_health(void);


// Handlers
#define ekf_aw_update_Q_accel(_v) { \
ekf_aw_params.Q_accel = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_gyro(_v) { \
ekf_aw_params.Q_gyro = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_mu(_v) { \
ekf_aw_params.Q_mu = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_Q_k(_v) { \
ekf_aw_params.Q_k = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_gnd(_v) { \
ekf_aw_params.R_V_gnd = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_x(_v) { \
ekf_aw_params.R_accel_filt[0] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_y(_v) { \
ekf_aw_params.R_accel_filt[1] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_accel_filt_z(_v) { \
ekf_aw_params.R_accel_filt[2] = _v; \
ekf_aw_update_params(); \
}

#define ekf_aw_update_R_V_pitot(_v) { \
ekf_aw_params.R_V_pitot = _v; \
ekf_aw_update_params(); \
}

#ifdef __cplusplus
}
#endif

#endif /* EKF_AW_H */
482 changes: 482 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.c
View file

Large diffs are not rendered by default.

118 changes: 118 additions & 0 deletions sw/airborne/modules/meteo/ekf_aw_wrapper.h
View file
@@ -0,0 +1,118 @@
#ifndef EKF_AW_WRAPPER_H
#define EKF_AW_WRAPPER_H

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_geodetic_float.h"
#include "modules/meteo/ekf_aw.h"


// EKF structure
struct ekfAw {

// States
struct NedCoor_f wind;
struct NedCoor_f V_body;
struct NedCoor_f offset;

// Inputs
struct FloatVect3 acc; ///< Last accelerometer measurements
struct FloatRates gyro; ///< Last gyroscope measurements
struct FloatEulers euler; /// Euler angles

#define EKF_AW_RPM_HOVER_NUM 4
int32_t last_RPM_hover[EKF_AW_RPM_HOVER_NUM]; // Value obtained from ABI Callback
int32_t last_RPM_pusher; // Value obtained from ABI Callback
float RPM_hover[EKF_AW_RPM_HOVER_NUM]; /// Hover motor RPM
float RPM_pusher; /// Pusher motor RPM
float skew; /// Skew
float elevator_angle;


// Measurements
struct FloatVect3 Vg_NED; /// Ground Speed
struct FloatVect3 acc_filt; ///< Last accelerometer measurements
float V_pitot; /// Pitot tube airspeed

// Innovation
struct FloatVect3 innov_V_gnd;
struct FloatVect3 innov_acc_filt;
float innov_V_pitot;

// Covariance
float meas_cov[7];
float state_cov[9];
float process_cov[12];

// Forces
float fuselage_force[3];
float wing_force[3];
float elevator_force[3];
float hover_force[3];
float pusher_force[3];

// Other
bool reset;
bool in_air;
struct NedCoor_f wind_guess;
struct NedCoor_f offset_guess;
struct ekfHealth health;
uint64_t internal_clock;
uint64_t time_last_on_gnd;
uint64_t time_last_in_air;
bool override_start;
bool override_quick_convergence;

};

extern void ekf_aw_wrapper_init(void);
extern void ekf_aw_wrapper_periodic(void);
extern void ekf_aw_wrapper_fetch(void);

extern void set_in_air_status(bool);


extern float tau_filter_high;
extern float tau_filter_low;

extern struct ekfAw ekf_aw;

// Handlers
#define ekf_aw_wrapper_reset(_v) { \
ekf_aw.reset = false; \
ekf_aw_reset(); \
ekf_aw_reset_health(); \
}

#define ekf_aw_wrapper_set_wind_N(_v) { \
ekf_aw.wind_guess.x = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_E(_v) { \
ekf_aw.wind_guess.y = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_wind_D(_v) { \
ekf_aw.wind_guess.z = _v; \
ekf_aw_set_wind(&ekf_aw.wind_guess); \
}

#define ekf_aw_wrapper_set_offset_x(_v) { \
ekf_aw.offset_guess.x = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_y(_v) { \
ekf_aw.offset_guess.y = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#define ekf_aw_wrapper_set_offset_z(_v) { \
ekf_aw.offset_guess.z = _v; \
ekf_aw_set_offset(&ekf_aw.offset_guess); \
}

#endif /* EKF_AW_WRAPPER_H */
150 changes: 150 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rot_wing_automation.c
View file
@@ -0,0 +1,150 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@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/rot_wing_drone/rot_wing_automation.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Fucntions to automate the navigation and guidance of the rotating wing drone
*/

#include "modules/rot_wing_drone/rot_wing_automation.h"
#include "state.h"
#include "modules/datalink/telemetry.h"

#ifndef ROT_WING_AUTOMATION_TRANS_ACCEL
#define ROT_WING_AUTOMATION_TRANS_ACCEL 1.0
#endif

#ifndef ROT_WING_AUTOMATION_TRANS_DECEL
#define ROT_WING_AUTOMATION_TRANS_DECEL 0.5
#endif

#ifndef ROT_WING_AUTOMATION_TRANS_LENGTH
#define ROT_WING_AUTOMATION_TRANS_LENGTH 200.0
#endif

#ifndef ROT_WING_AUTOMATION_TRANS_AIRSPEED
#define ROT_WING_AUTOMATION_TRANS_AIRSPEED 15.0
#endif

struct rot_wing_automation rot_wing_a;

// declare function
inline void update_waypoint_rot_wing_automation(uint8_t wp_id, struct FloatVect3 * target_ned);

void init_rot_wing_automation(void)
{
rot_wing_a.trans_accel = ROT_WING_AUTOMATION_TRANS_ACCEL;
rot_wing_a.trans_decel = ROT_WING_AUTOMATION_TRANS_DECEL;
rot_wing_a.trans_length = ROT_WING_AUTOMATION_TRANS_LENGTH;
rot_wing_a.trans_airspeed = ROT_WING_AUTOMATION_TRANS_AIRSPEED;
rot_wing_a.transitioned = false;
}

// periodic function
void periodic_rot_wing_automation(void)
{
float airspeed = stateGetAirspeed_f();
if (airspeed > rot_wing_a.trans_airspeed)
{
rot_wing_a.transitioned = true;
} else {
rot_wing_a.transitioned = false;
}
}

// Update a waypoint such that you can see on the GCS where the drone wants to go
void update_waypoint_rot_wing_automation(uint8_t wp_id, struct FloatVect3 * target_ned) {

// Update the waypoint
struct EnuCoor_f target_enu;
ENU_OF_TO_NED(target_enu, *target_ned);
waypoint_set_enu(wp_id, &target_enu);

// Send waypoint update every half second
RunOnceEvery(100/2, {
// Send to the GCS that the waypoint has been moved
DOWNLINK_SEND_WP_MOVED_ENU(DefaultChannel, DefaultDevice, &wp_id,
&waypoints[wp_id].enu_i.x,
&waypoints[wp_id].enu_i.y,
&waypoints[wp_id].enu_i.z);
} );
}

// Function to visualize from flightplan, call repeadely
void rot_wing_vis_transition(uint8_t wp_transition_id, uint8_t wp_decel_id, uint8_t wp_end_id)
{
//state eulers in zxy order
struct FloatEulers eulers_zxy;

// get heading and cos and sin of heading
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
float psi = eulers_zxy.psi;
float cpsi = cosf(psi);
float spsi = sinf(psi);

// get airspeed
float airspeed = stateGetAirspeed_f();
// get groundspeed
float ground_speed = stateGetHorizontalSpeedNorm_f();

// get drone position
struct NedCoor_f *drone_pos = stateGetPositionNed_f();

// Move reference waypoints
// Move end transition waypoint at the correct length
struct FloatVect3 end_transition_rel_pos;
VECT3_COPY(end_transition_rel_pos, *drone_pos);
end_transition_rel_pos.x = cpsi * rot_wing_a.trans_length;
end_transition_rel_pos.y = spsi * rot_wing_a.trans_length;
struct FloatVect3 end_transition_pos;
VECT3_SUM(end_transition_pos, end_transition_rel_pos, *drone_pos);
end_transition_pos.z = drone_pos->z;
update_waypoint_rot_wing_automation(wp_end_id, &end_transition_pos);

// Move transition waypoint
float airspeed_error = rot_wing_a.trans_airspeed - airspeed;
float transition_time = airspeed_error / rot_wing_a.trans_accel;
float average_ground_speed = ground_speed + airspeed_error / 2.;
float transition_distance = average_ground_speed * transition_time;

struct FloatVect3 transition_rel_pos;
VECT3_COPY(transition_rel_pos, *drone_pos);
transition_rel_pos.x = cpsi * transition_distance;
transition_rel_pos.y = spsi * transition_distance;
struct FloatVect3 transition_pos;
VECT3_SUM(transition_pos, transition_rel_pos, *drone_pos);
transition_pos.z = drone_pos->z;
update_waypoint_rot_wing_automation(wp_transition_id, &transition_pos);

// Move decel point
float final_groundspeed = ground_speed + airspeed_error;
float decel_time = final_groundspeed / rot_wing_a.trans_decel;
float decel_distance = (final_groundspeed / 2.) * decel_time;
float decel_distance_from_drone = rot_wing_a.trans_length - decel_distance;

struct FloatVect3 decel_rel_pos;
VECT3_COPY(decel_rel_pos, *drone_pos);
decel_rel_pos.x = cpsi * decel_distance_from_drone;
decel_rel_pos.y = spsi * decel_distance_from_drone;
struct FloatVect3 decel_pos;
VECT3_SUM(decel_pos, decel_rel_pos, *drone_pos);
decel_pos.z = drone_pos->z;
update_waypoint_rot_wing_automation(wp_decel_id, &decel_pos);
}
46 changes: 46 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rot_wing_automation.h
View file
@@ -0,0 +1,46 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@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/rot_wing_drone/rot_wing_automation.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Fucntions to automate the navigation and guidance of the rotating wing drone
*/

#ifndef ROT_WING_AUTOMATION_H
#define ROT_WING_AUTOMATION_H

#include "std.h"
#include "math/pprz_algebra_float.h"

extern void init_rot_wing_automation(void);
extern void periodic_rot_wing_automation(void);
extern void rot_wing_vis_transition(uint8_t wp_transition_id, uint8_t wp_decel_id, uint8_t wp_end_id);

struct rot_wing_automation {
float trans_accel;
float trans_decel;
float trans_length;
float trans_airspeed;
bool transitioned;
};

extern struct rot_wing_automation rot_wing_a;

#endif // ROT_WING_AUTOMATION_H
389 changes: 389 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rotwing_state.c
View file
@@ -0,0 +1,389 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@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.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@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.
*/

#include "modules/rot_wing_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/rot_wing_drone/wing_rotation_controller_v3b.h"
#include "modules/actuators/actuators.h"
#include "modules/core/abi.h"

struct RotwingState rotwing_state;

// Quad state identification
#ifndef ROTWING_MIN_SKEW_ANGLE_DEG_QUAD
#define ROTWING_MIN_SKEW_ANGLE_DEG_QUAD 10.0
#endif
#ifndef ROTWING_MIN_SKEW_ANGLE_COUNTER
#define ROTWING_MIN_SKEW_ANGLE_COUNTER 10 // Minimum number of loops the skew angle is below ROTWING_MIN_SKEW_ANGLE_COUNTER
#endif

// Skewing state identification
#ifndef ROTWING_SKEWING_COUNTER
#define ROTWING_SKEWING_COUNTER 10 // Minimum number of loops the skew angle is in between QUAD and FW
#endif

// Half skew state identification
#ifndef ROTWING_HALF_SKEW_ANGLE_DEG
#define ROTWING_HALF_SKEW_ANGLE_DEG 55.0
#endif
#ifndef ROTWING_HALF_SKEW_ANGLE_RANG
#define ROTWING_HALF_SKEW_ANGLE_HALF_RANGE 10.0
#endif
#ifndef ROTWING_HALF_SKEW_COUNTER
#define ROTWING_HALF_SKEW_COUNTER 10 // Minimum number of loops the skew angle is at HALF_SKEW_ANGLE_DEG +/- ROTWING_HALF_SKEW_ANGLE_HALF_RANGE to trigger ROTWING_HALF_SKEW_ANGLE state
#endif

// FW state identification
#ifndef ROTWING_MIN_FW_SKEW_ANGLE_DEG
#define ROTWING_MIN_FW_SKEW_ANGLE_DEG 80.0 // Minimum wing angle to fly in fixed wing state
#endif
#ifndef ROTWING_MIN_FW_COUNTER
#define ROTWING_MIN_FW_COUNTER 10 // Minimum number of loops the skew angle is above the MIN_FW_SKEW_ANGLE
#endif

// FW idle state identification
#ifndef ROTWING_MIN_THRUST_IDLE
#define ROTWING_MIN_THRUST_IDLE 100
#endif
#ifndef ROTWING_MIN_THRUST_IDLE_COUNTER
#define ROTWING_MIN_THRUST_IDLE_COUNTER 10
#endif

// FW hov mot off state identification
#ifndef ROTWING_HOV_MOT_OFF_RPM_TH
#define ROTWING_HOV_MOT_OFF_RPM_TH 50
#endif
#ifndef ROTWING_HOV_MOT_OFF_COUNTER
#define ROTWING_HOV_MOT_OFF_COUNTER 10
#endif

#ifndef ROTWING_STATE_RPM_ID
#define ROTWING_STATE_RPM_ID ABI_BROADCAST
#endif
abi_event rotwing_state_rpm_ev;
static void rotwing_state_rpm_cb(uint8_t sender_id, struct rpm_act_t * rpm_message, uint8_t num_act);
#define ROTWING_STATE_NUM_HOVER_RPM 4
int32_t rotwing_state_hover_rpm[ROTWING_STATE_NUM_HOVER_RPM] = {0, 0, 0, 0};

bool rotwing_state_force_quad = false;

uint8_t rotwing_state_hover_counter = 0;
uint8_t rotwing_state_skewing_counter = 0;
uint8_t rotwing_state_fw_counter = 0;
uint8_t rotwing_state_fw_idle_counter = 0;
uint8_t rotwing_state_fw_m_off_counter = 0;

inline void rotwing_check_set_current_state(void);
inline void rotwing_update_desired_state(void);
inline void rotwing_switch_state(void);

#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID,
&rotwing_state.current_state,
&rotwing_state.desired_state);
}
#endif // PERIODIC_TELEMETRY

void init_rotwing_state(void)
{
// Bind ABI messages
AbiBindMsgRPM(ROTWING_STATE_RPM_ID, &rotwing_state_rpm_ev, rotwing_state_rpm_cb);

// Start the drone in a desired hover state
rotwing_state.current_state = ROTWING_STATE_HOVER;
rotwing_state.desired_state = ROTWING_STATE_HOVER;

#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTATING_WING_STATE, send_rotating_wing_state);
#endif
}

void periodic_rotwing_state(void)
{
// Check and set the current state
rotwing_check_set_current_state();

// Check and update desired state
rotwing_update_desired_state();

// Check difference with desired state
// rotwing_switch_state();

//TODO: incorparate motor active / disbaling depending on called flight state
// Switch on motors if flight mode is attitude
if (guidance_h.mode == GUIDANCE_H_MODE_ATTITUDE) {
bool_disable_hover_motors = false;
} else if (guidance_h.mode == GUIDANCE_H_MODE_FORWARD) {
bool_disable_hover_motors = false;
}
}

// Function to request a state
void request_rotwing_state(uint8_t state)
{
if (state == ROTWING_STATE_HOVER) {
rotwing_state.current_state = ROTWING_STATE_HOVER;
} else if (state == ROTWING_STATE_FW) {
rotwing_state.current_state = ROTWING_STATE_FW;
}
}

void rotwing_check_set_current_state(void)
{
// if !in_flight, set state to hover
if (!autopilot.in_flight) {
rotwing_state_hover_counter = 0;
rotwing_state_skewing_counter = 0;
rotwing_state_fw_counter = 0;
rotwing_state_fw_idle_counter = 0;
rotwing_state_fw_m_off_counter = 0;
rotwing_state.current_state = ROTWING_STATE_HOVER;
return;
}

// States can be checked according to wing angle sensor, setpoints .....
uint8_t prev_state = rotwing_state.current_state;
switch (prev_state) {
case ROTWING_STATE_HOVER:
// Check if state needs to be set to skewing
if (wing_rotation.wing_angle_deg > ROTWING_MIN_SKEW_ANGLE_DEG_QUAD)
{
rotwing_state_skewing_counter++;
} else {
rotwing_state_skewing_counter = 0;
}

// Switch state if necessary
if (rotwing_state_skewing_counter > ROTWING_MIN_SKEW_ANGLE_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_SKEWING;
rotwing_state_skewing_counter = 0;
}
break;

case ROTWING_STATE_SKEWING:
// Check if state needs to be set to hover
if (wing_rotation.wing_angle_deg < ROTWING_MIN_SKEW_ANGLE_DEG_QUAD)
{
rotwing_state_hover_counter++;
} else {
rotwing_state_hover_counter = 0;
}

// Check if state needs to be set to fixed wing
if (wing_rotation.wing_angle_deg > ROTWING_MIN_FW_SKEW_ANGLE_DEG)
{
rotwing_state_fw_counter++;
} else {
rotwing_state_fw_counter = 0;
}

// Switch state if necessary
if (rotwing_state_hover_counter > ROTWING_MIN_SKEW_ANGLE_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_HOVER;
rotwing_state_hover_counter = 0;
}

if (rotwing_state_fw_counter > ROTWING_MIN_FW_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_FW;
rotwing_state_fw_counter = 0;
}
break;

case ROTWING_STATE_FW:
// Check if state needs to be set to skewing
if (wing_rotation.wing_angle_deg < ROTWING_MIN_FW_SKEW_ANGLE_DEG) {
rotwing_state_skewing_counter++;
} else {
rotwing_state_skewing_counter = 0;
}

// Check if state needs to be set to fixed wing with hover motors idle (If hover thrust below threshold)
if (stabilization_cmd[COMMAND_THRUST] < ROTWING_MIN_THRUST_IDLE) {
rotwing_state_fw_idle_counter++;
} else {
rotwing_state_fw_idle_counter = 0;
}

// Switch state if necessary
if (rotwing_state_skewing_counter > ROTWING_MIN_FW_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_SKEWING;
rotwing_state_skewing_counter = 0;
rotwing_state_fw_idle_counter = 0;
} else if (rotwing_state_fw_idle_counter > ROTWING_MIN_THRUST_IDLE_COUNTER
&& rotwing_state_skewing_counter == 0) {
rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_IDLE;
rotwing_state_skewing_counter = 0;
rotwing_state_fw_idle_counter = 0;
}
break;

case ROTWING_STATE_FW_HOV_MOT_IDLE:
// Check if state needs to be set to fixed wing with hover motors activated
if (stabilization_cmd[COMMAND_THRUST] > ROTWING_MIN_THRUST_IDLE) {
rotwing_state_fw_counter++;
} else {
rotwing_state_fw_counter = 0;
}

// Check if state needs to be set to fixed wing with hover motors off (if zero rpm on hover motors)
if (rotwing_state_hover_rpm[0] == 0
&& rotwing_state_hover_rpm[1] == 0
&& rotwing_state_hover_rpm[2] == 0
&& rotwing_state_hover_rpm[3] == 0) {
rotwing_state_fw_m_off_counter++;
} else {
rotwing_state_fw_m_off_counter = 0;
}

// Switch state if necessary
if (rotwing_state_fw_counter > ROTWING_MIN_THRUST_IDLE_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_FW;
rotwing_state_fw_counter = 0;
rotwing_state_fw_m_off_counter = 0;
} else if (rotwing_state_fw_m_off_counter > ROTWING_HOV_MOT_OFF_COUNTER
&& rotwing_state_fw_counter == 0) {
rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_OFF;
rotwing_state_fw_counter = 0;
rotwing_state_fw_m_off_counter = 0;
}
break;

case ROTWING_STATE_FW_HOV_MOT_OFF:
// Check if state needs to be set to fixed wing with hover motors idle (if rpm on hover motors)
if (rotwing_state_hover_rpm[0] > ROTWING_HOV_MOT_OFF_RPM_TH
&& rotwing_state_hover_rpm[1] > ROTWING_HOV_MOT_OFF_RPM_TH
&& rotwing_state_hover_rpm[2] > ROTWING_HOV_MOT_OFF_RPM_TH
&& rotwing_state_hover_rpm[3] > ROTWING_HOV_MOT_OFF_RPM_TH) {
rotwing_state_fw_idle_counter++;
} else {
rotwing_state_fw_idle_counter = 0;
}

// Switch state if necessary
if (rotwing_state_fw_counter > ROTWING_MIN_THRUST_IDLE_COUNTER) {
rotwing_state.current_state = ROTWING_STATE_FW_HOV_MOT_IDLE;
rotwing_state_fw_idle_counter = 0;
}
break;

default:
break;
}
}

void rotwing_update_desired_state(void)
{
// If force_forward and nav selected, then the desired state if FW with hover motors off;
switch (guidance_h.mode) {
case GUIDANCE_H_MODE_NAV:
if (rotwing_state_force_quad) {
rotwing_state.desired_state = ROTWING_STATE_HOVER;
} else if (force_forward) {
rotwing_state.desired_state = ROTWING_STATE_FW_HOV_MOT_OFF;
} else {
rotwing_state.desired_state = ROTWING_STATE_FREE;
}
break;

case GUIDANCE_H_MODE_FORWARD:
rotwing_state.desired_state = ROTWING_STATE_FW_HOV_MOT_OFF;
break;

case GUIDANCE_H_MODE_ATTITUDE:
rotwing_state.desired_state = ROTWING_STATE_HOVER;
break;

default:
rotwing_state.desired_state = ROTWING_STATE_FREE;
break;
}
}

// Function that handles settings for switching state(s)
void rotwing_switch_state(void)
{
switch (rotwing_state.current_state) {
case ROTWING_STATE_HOVER:
if (rotwing_state.desired_state > ROTWING_STATE_HOVER) {
// if in hover state, but higher state requested, switch on wing_rotation scheduler
wing_rotation.airspeed_scheduling = true;
} else {
// if hover state desired and at the state, fix wing in quad
wing_rotation.airspeed_scheduling = false;
wing_rotation.wing_angle_deg_sp = 0;
}
break;

case ROTWING_STATE_SKEWING:
if (rotwing_state.desired_state > ROTWING_STATE_SKEWING) {
// Keep wing rotation scheduler on if skewing to a higher state
wing_rotation.airspeed_scheduling = true;
} else {
// go back to quad state if fulfilling conditions for skewing back (motors on, skewing back not too fast).
}
break;

case ROTWING_STATE_FW:
break;

case ROTWING_STATE_FW_HOV_MOT_IDLE:
if (rotwing_state.desired_state > ROTWING_STATE_FW_HOV_MOT_IDLE) {
// Switch off hover motors if in idle and desired state is higher
bool_disable_hover_motors = true;
} else if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_IDLE) {
// Allow hover motors to generate thrust when desired state is lower than current state
hover_motors_active = true;
} else {
// if idle desired and already at idle, let motors spin idle
bool_disable_hover_motors = false;
hover_motors_active = false;
}
break;

case ROTWING_STATE_FW_HOV_MOT_OFF:
if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_OFF) {
// Switch on the hover motors when going to a lower state
bool_disable_hover_motors = false;
}
wing_rotation.airspeed_scheduling = false;
wing_rotation.wing_angle_deg_sp = 90;
break;
}
}

static void rotwing_state_rpm_cb(uint8_t __attribute__((unused)) sender_id, struct rpm_act_t UNUSED * rpm_message, uint8_t UNUSED num_act_message)
{
for (int i=0; i<num_act_message; i++) {
// Sanity check that index is valid
if (rpm_message[i].actuator_idx < ROTWING_STATE_NUM_HOVER_RPM){
rotwing_state_hover_rpm[rpm_message->actuator_idx] = rpm_message->rpm;
}
}
}
53 changes: 53 additions & 0 deletions sw/airborne/modules/rot_wing_drone/rotwing_state.h
View file
@@ -0,0 +1,53 @@
/*
* Copyright (C) 2023 Dennis van Wijngaarden <D.C.vanWijngaarden@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.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@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

struct RotwingState {
uint8_t current_state;
uint8_t desired_state;
};

extern struct RotwingState rotwing_state;

extern bool rotwing_state_force_quad;

extern void init_rotwing_state(void);
extern void periodic_rotwing_state(void);
extern void request_rotwing_state(uint8_t state);

#endif // ROTWING_STATE_H
262 changes: 262 additions & 0 deletions sw/airborne/modules/rot_wing_drone/wing_rotation_controller_v3b.c
View file
@@ -0,0 +1,262 @@
/*
* Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@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/rot_wing_drone/wing_rotation_controller_v3b.c"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Module to control wing rotation servo command based on prefered angle setpoint
*/

#include "modules/rot_wing_drone/wing_rotation_controller_v3b.h"
#include "modules/radio_control/radio_control.h"

#include "state.h"

#include <stdlib.h>
#include "mcu_periph/adc.h"

/*
#ifndef WING_ROTATION_SERVO_IDX
#error "No WING_ROTATION_SERVO_IDX defined"
#endif
*/

#if !USE_NPS

#ifndef ADC_CHANNEL_WING_ROTATION_POSITION
#define ADC_CHANNEL_WING_ROTATION_POSITION ADC_5
#endif

#ifndef ADC_CHANNEL_WING_ROTATION_POSITION_NB_SAMPLES
#define ADC_CHANNEL_WING_ROTATION_POSITION_NB_SAMPLES 16
#endif

#endif // !USE_NPS

#ifndef WING_ROTATION_POSITION_ADC_0
#error "No WING_ROTATION_POSITION_ADC_0 defined"
#endif

#ifndef WING_ROTATION_POSITION_ADC_90
#error "No WING_ROTATION_POSITION_ADC_90 defined"
#endif

#ifndef WING_ROTATION_FIRST_DYN
#define WING_ROTATION_FIRST_DYN 0.001
#endif

#ifndef WING_ROTATION_SECOND_DYN
#define WING_ROTATION_SECOND_DYN 0.003
#endif

// Parameters
struct wing_rotation_controller wing_rotation;

bool in_transition = false;

#if !USE_NPS
static struct adc_buf buf_wing_rot_pos;
#endif

// Inline functions
inline void wing_rotation_to_rad(void);
inline void wing_rotation_update_sp(void);
inline void wing_rotation_compute_pprz_cmd(void);

// Telemetry
#if PERIODIC_TELEMETRY
#include "modules/datalink/telemetry.h"
static void send_rot_wing_controller(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_ROT_WING_CONTROLLER(trans, dev, AC_ID,
&wing_rotation.wing_angle_deg,
&wing_rotation.wing_angle_deg_sp,
&wing_rotation.adc_wing_rotation,
&wing_rotation.servo_pprz_cmd);
}
#endif

void wing_rotation_init(void)
{
// your init code here
#if !USE_NPS
adc_buf_channel(ADC_CHANNEL_WING_ROTATION_POSITION, &buf_wing_rot_pos, ADC_CHANNEL_WING_ROTATION_POSITION_NB_SAMPLES);
#endif

// Init wing_rotation_controller struct
wing_rotation.servo_pprz_cmd = 0;
wing_rotation.adc_wing_rotation = 0;
wing_rotation.wing_angle_rad = 0;
wing_rotation.wing_angle_deg = 0;
wing_rotation.wing_angle_rad_sp = 0;
wing_rotation.wing_angle_deg_sp = 0;
wing_rotation.wing_rotation_speed = 0;
wing_rotation.wing_angle_virtual_deg_sp = 45;
wing_rotation.wing_rotation_first_order_dynamics = WING_ROTATION_FIRST_DYN;
wing_rotation.wing_rotation_second_order_dynamics = WING_ROTATION_SECOND_DYN;
wing_rotation.adc_wing_rotation_range = WING_ROTATION_POSITION_ADC_90 - WING_ROTATION_POSITION_ADC_0;
wing_rotation.airspeed_scheduling = false;
wing_rotation.transition_forward = false;
wing_rotation.forward_airspeed = 18.;

// Set wing angle to current wing angle
wing_rotation.initialized = false;
wing_rotation.init_loop_count = 0;

#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROT_WING_CONTROLLER, send_rot_wing_controller);
#endif
}

void wing_rotation_periodic(void)
{
// your periodic code here.
// freq = 1.0 Hz

// After 5 loops, set current setpoint and enable wing_rotation
if (!wing_rotation.initialized) {
wing_rotation.init_loop_count += 1;
if (wing_rotation.init_loop_count > 4) {
wing_rotation.initialized = true;
wing_rotation.wing_angle_rad_sp = M_PI * 0.25;
wing_rotation.wing_angle_deg_sp = wing_rotation.wing_angle_rad_sp / M_PI * 180.;
}
}
}

void wing_rotation_event(void)
{
// First check if safety switch is triggered
#ifdef WING_ROTATION_RESET_RADIO_CHANNEL
// Update wing_rotation deg setpoint when RESET switch triggered
if (radio_control.values[WING_ROTATION_RESET_RADIO_CHANNEL] > 1750)
{
wing_rotation.airspeed_scheduling = false;
wing_rotation.wing_angle_deg_sp = 0;
}
#endif

if (guidance_h.mode == GUIDANCE_H_MODE_FORWARD)
{
set_wing_rotation_scheduler(true);
} else if (guidance_h.mode == GUIDANCE_H_MODE_ATTITUDE)
{
set_wing_rotation_scheduler(false);
}

if (!wing_rotation.airspeed_scheduling) {
wing_rotation.transition_forward = false;
}

// Update Wing position sensor
wing_rotation_to_rad();

// Run control if initialized
if (wing_rotation.initialized) {

if (wing_rotation.airspeed_scheduling)
{
float wing_angle_scheduled_sp_deg = 0;
float airspeed = stateGetAirspeed_f();
if (airspeed < 8) {
in_transition = false;
wing_angle_scheduled_sp_deg = 0;
} else if (airspeed < 10 && in_transition) {
wing_angle_scheduled_sp_deg = 55;
} else if (airspeed > 10) {
wing_angle_scheduled_sp_deg = ((airspeed - 10.)) / 4. * 35. + 55.;
in_transition = true;
} else {
wing_angle_scheduled_sp_deg = 0;
}

Bound(wing_angle_scheduled_sp_deg, 0., 90.)
wing_rotation.wing_angle_deg_sp = wing_angle_scheduled_sp_deg;

}

wing_rotation_update_sp();

//int32_t servo_pprz_cmd; // Define pprz cmd

// Control the wing rotation position.
wing_rotation_compute_pprz_cmd();
//servo_pprz_cmd = wing_rotation.wing_angle_deg_sp / 90 * MAX_PPRZ;
//Bound(servo_pprz_cmd, 0, MAX_PPRZ);

//wing_rotation.servo_pprz_cmd = servo_pprz_cmd;
}
}

void wing_rotation_to_rad(void)
{
#if !USE_NPS
wing_rotation.adc_wing_rotation = buf_wing_rot_pos.sum / buf_wing_rot_pos.av_nb_sample;

wing_rotation.wing_angle_deg = 0.00247111 * (float)wing_rotation.adc_wing_rotation - 25.635294;
wing_rotation.wing_angle_rad = wing_rotation.wing_angle_deg / 180. * M_PI;

#else
// Copy setpoint as actual angle in simulation
wing_rotation.wing_angle_deg = wing_rotation.wing_angle_virtual_deg_sp;
wing_rotation.wing_angle_rad = wing_rotation.wing_angle_virtual_deg_sp / 180. * M_PI;
#endif
}

void wing_rotation_update_sp(void)
{
wing_rotation.wing_angle_rad_sp = wing_rotation.wing_angle_deg_sp / 180. * M_PI;
}

void wing_rotation_compute_pprz_cmd(void)
{
float angle_error = wing_rotation.wing_angle_deg_sp - wing_rotation.wing_angle_virtual_deg_sp;
float speed_sp = wing_rotation.wing_rotation_first_order_dynamics * angle_error;
float speed_error = speed_sp - wing_rotation.wing_rotation_speed;
wing_rotation.wing_rotation_speed += wing_rotation.wing_rotation_second_order_dynamics * speed_error;
wing_rotation.wing_angle_virtual_deg_sp += wing_rotation.wing_rotation_speed;

#if !USE_NPS
int32_t servo_pprz_cmd; // Define pprz cmd
servo_pprz_cmd = (int32_t)(wing_rotation.wing_angle_virtual_deg_sp / 90. * (float)MAX_PPRZ);
Bound(servo_pprz_cmd, 0, MAX_PPRZ);

wing_rotation.servo_pprz_cmd = servo_pprz_cmd;
#else
int32_t servo_pprz_cmd; // Define pprz cmd
servo_pprz_cmd = (int32_t)(wing_rotation.wing_angle_deg_sp / 90. * (float)MAX_PPRZ);
Bound(servo_pprz_cmd, 0, MAX_PPRZ);

wing_rotation.servo_pprz_cmd = servo_pprz_cmd;
actuators_pprz[INDI_NUM_ACT] = servo_pprz_cmd;
#endif
}

// Function to call in flightplan to switch scheduler on or off
bool set_wing_rotation_scheduler(bool rotation_scheduler_on)
{
if (rotation_scheduler_on)
{
wing_rotation.airspeed_scheduling = true;
} else {
wing_rotation.airspeed_scheduling = false;
wing_rotation.wing_angle_deg_sp = 0;
}
return false;
}
63 changes: 63 additions & 0 deletions sw/airborne/modules/rot_wing_drone/wing_rotation_controller_v3b.h
View file
@@ -0,0 +1,63 @@
/*
* Copyright (C) 2022 Dennis van Wijngaarden <D.C.vanWijngaarden@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/rot_wing_drone/wing_rotation_controller.h"
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
* Module to control wing rotation servo command based on prefered angle setpoint
*/

#ifndef WING_ROTATION_CONTROLLER_V3B_H
#define WING_ROTATION_CONTROLLER_V3B_H

#include "std.h"

extern void wing_rotation_init(void);
extern void wing_rotation_periodic(void);
extern void wing_rotation_event(void);
extern bool set_wing_rotation_scheduler(bool rotation_scheduler_on);

// Paramaters
struct wing_rotation_controller {
int32_t servo_pprz_cmd;
uint16_t adc_wing_rotation;
int16_t adc_wing_rotation_range;
float wing_angle_rad;
float wing_angle_deg;
float wing_angle_rad_sp;
float wing_angle_deg_sp;
float wing_rotation_speed;
float wing_angle_virtual_deg_sp;
float wing_rotation_first_order_dynamics;
float wing_rotation_second_order_dynamics;
bool initialized;
uint8_t init_loop_count;
bool airspeed_scheduling;
bool transition_forward;
float forward_airspeed;
};

extern float wing_rotation_sched_as_1;
extern float wing_rotation_sched_as_2;
extern float wing_rotation_sched_as_3;
extern float wing_rotation_sched_as_4;

extern struct wing_rotation_controller wing_rotation;

#endif // WING_ROTATION_CONTROLLER_V3B_H
2 changes: 1 addition & 1 deletion sw/ext/Makefile
View file
Expand Up @@ -33,7 +33,7 @@ EXT_DIR=$(PAPARAZZI_SRC)/sw/ext
include $(PAPARAZZI_SRC)/conf/Makefile.arm-embedded-toolchain


all: libopencm3 luftboot chibios fatfs libsbp TRICAL hacl-c key_generator rustlink ecl matrix
all: libopencm3 luftboot chibios fatfs libsbp TRICAL hacl-c key_generator rustlink ecl matrix mavlink

# update (and init if needed) all submodules
update_submodules:
Expand Down