| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 */ |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,51 @@ | ||
| /* | ||
| * 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 void init_eff_scheduling(void); | ||
| extern void event_eff_scheduling(void); | ||
|
|
||
| #endif // CTRL_EFF_SCHED_ROT_WING_V3_H |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 */ |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 */ |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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); | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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_rot_wing.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; | ||
| } | ||
| } | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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; | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,275 @@ | ||
| import numpy as np | ||
| import scipy as sp | ||
| from scipy import signal | ||
| import matplotlib.pyplot as plt | ||
| import pandas as pd | ||
|
|
||
| import sys | ||
| from os import path, getenv | ||
|
|
||
| class CtrlEffEst(object): | ||
| def __init__(self, parsed_log, first_order_dyn_list, second_order_filter_cutoff, min_max_pwm_list, is_servo, actuator_indices = None, fs=500.): | ||
| ''' | ||
| Initialization function of control effectiveness estimator | ||
| ''' | ||
| self.data_indi = parsed_log.get_message_dict('STAB_ATTITUDE_FULL_INDI') | ||
| self.data_actuators = parsed_log.get_message_dict('ACTUATORS') | ||
| self.data_doublet = parsed_log.get_message_dict('DOUBLET') | ||
| self.data_airdata = parsed_log.get_message_dict('AIR_DATA') | ||
| # self.data_wing_rot = parsed_log.get_message_dict('ROT_WING_CONTROLLER') | ||
|
|
||
| self.N_act = len(actuator_indices) | ||
| self.actuator_indices = actuator_indices | ||
|
|
||
| # Copy variables | ||
| self.first_order_dyn_list = first_order_dyn_list # First order actuator dynamics constant | ||
| self.second_order_filter_cutoff= second_order_filter_cutoff # Butterworth second order cutoff frequency [Hz] | ||
| self.min_max_pwm_list = min_max_pwm_list # 2d list with lower and upper bound pwm signals | ||
| self.is_servo = is_servo # bool list giving if actuator is servo (aerodynamic surface control) | ||
| self.fs = fs # Sample frequency [Hz] | ||
| self.dt = 1./fs # Sample time [s] | ||
|
|
||
| # Check if first order dyn list is the same size as self.N_act | ||
| if (self.N_act != len(self.first_order_dyn_list)): | ||
| print('ERROR: length of actuator inputs not corresponding to first order actuator dynamics') | ||
|
|
||
| def get_effectiveness_values(self, plot = True): | ||
| ''' | ||
| Returns effectiveness values for each actuator and axis | ||
| ''' | ||
| # Order data arrays | ||
| t_indi = np.array(self.data_indi['t']) | ||
| t_act = np.array(self.data_actuators['t']) | ||
| t_doublet = np.array(self.data_doublet['t']) | ||
| t_airdata = np.array(self.data_airdata['t']) | ||
| # t_rot_wing_controller = np.array(self.data_wing_rot['t']) | ||
|
|
||
| doublet_activated = np.array(self.data_doublet['active']['data']) | ||
| doublet_axis = np.array(self.data_doublet['axis']['data']) | ||
| pwm_original = np.array(self.data_actuators['values']['data'])[:,self.actuator_indices] | ||
| pwm_sliced = [] | ||
| airspeed = np.array(self.data_airdata['airspeed']['data']) | ||
| # wing_rot = np.deg2rad(np.array(self.data_wing_rot['wing_angle_deg']['data'])) | ||
| # 2nd order bUtterworth noise filter | ||
| b, a = sp.signal.butter(2, 0.1/(50/2), 'low', analog=False) | ||
| airspeed_filtered = sp.signal.lfilter(b, a, airspeed) | ||
|
|
||
|
|
||
| for i in range(self.N_act): | ||
| pwm_interp = np.interp(t_indi, t_act, pwm_original.T[i]) | ||
| pwm_sliced.append(pwm_interp) | ||
|
|
||
| pwm = np.array(pwm_sliced).T | ||
| # Convert pwm to command | ||
| min_pwm_array = np.array(self.min_max_pwm_list).T[0] | ||
| max_pwm_array = np.array(self.min_max_pwm_list).T[1] | ||
| pwm_range = max_pwm_array - min_pwm_array | ||
| cmd = (pwm - min_pwm_array) / pwm_range * 9600. | ||
|
|
||
| rate_p = np.array(self.data_indi['angular_rate_p']['data'])/180.*np.pi # rad/s | ||
| rate_q = np.array(self.data_indi['angular_rate_q']['data'])/180.*np.pi # rad/s | ||
| rate_r = np.array(self.data_indi['angular_rate_r']['data'])/180.*np.pi # rad/s | ||
| rates = np.array([rate_p,rate_q,rate_r]).T | ||
| acc_x = self.data_indi['body_accel_x']['data'] # m/s² | ||
| acc_z = self.data_indi['body_accel_z']['data'] # m/s² | ||
|
|
||
| # Apply actuator dynamics to commands | ||
| for i in range(self.N_act): | ||
| zi = sp.signal.lfilter_zi([self.first_order_dyn_list[i]], [1, -(1-self.first_order_dyn_list[i])]) | ||
| #filtered_cmd = sp.signal.lfilter([self.first_order_dyn_list[i]], [1, -(1-self.first_order_dyn_list[i])], cmd[:,i], zi=zi*cmd[:,i][0])[0] | ||
| filtered_cmd = sp.signal.lfilter([self.first_order_dyn_list[i]], [1, -(1-self.first_order_dyn_list[i])], cmd[:,i]) | ||
| if i == 0: | ||
| cmd_a_T = np.array([filtered_cmd]) # Transpose of cmd_a | ||
| else: | ||
| cmd_a_T = np.vstack((cmd_a_T, filtered_cmd)) | ||
|
|
||
| cmd_a = cmd_a_T.T | ||
|
|
||
| # 2nd order bUtterworth noise filter | ||
| b, a = sp.signal.butter(2, self.second_order_filter_cutoff/(self.fs/2), 'low', analog=False) | ||
|
|
||
| # Filter signals and cmds | ||
| #zi = np.array([sp.signal.lfilter_zi(b, a)]).T | ||
| rates_f = sp.signal.lfilter(b, a, rates, axis=0) | ||
| acc_x_f = sp.signal.lfilter(b, a, acc_x, axis=0) | ||
| acc_z_f = sp.signal.lfilter(b, a, acc_z, axis=0) | ||
| cmd_af = sp.signal.lfilter(b, a, cmd_a, axis=0) | ||
|
|
||
| # Apply finite difference methods to get anfular accelerarions | ||
| d_rates = (np.vstack((np.zeros((1,3)), np.diff(rates_f,1,axis=0)))*self.fs) | ||
| dd_rates = (np.vstack((np.zeros((1,3)), np.diff(d_rates,1,axis=0)))*self.fs) | ||
|
|
||
| dd_rates = np.vstack((dd_rates.T,acc_x_f)).T | ||
| dd_rates = np.vstack((dd_rates.T,acc_z_f)).T | ||
|
|
||
| d_cmd= (np.vstack((np.zeros((1,self.N_act)), np.diff(cmd_af,1,axis=0)))*self.fs) | ||
| dd_cmd = (np.vstack((np.zeros((1,self.N_act)), np.diff(d_cmd,1,axis=0)))*self.fs) | ||
|
|
||
| t = t_indi | ||
| # Construct A matrix | ||
| for i in range(len(self.actuator_indices)): | ||
| row = d_cmd[:,i] | ||
|
|
||
| if i == 0: | ||
| A = row | ||
| else: | ||
| A = np.vstack((A, row)) | ||
|
|
||
| A = A.T | ||
|
|
||
| # Check timespans when doublets activated | ||
| change_doublet_idx = np.where(doublet_activated[:-1] != doublet_activated[1:])[0] | ||
| t_change_doublet = t_doublet[change_doublet_idx] | ||
| #print(len(t_change_doublet)) | ||
|
|
||
| doublet_actuator = [] | ||
| eff_roll = [] | ||
| eff_pitch = [] | ||
| eff_yaw = [] | ||
| eff_acc_x = [] | ||
| eff_acc_z = [] | ||
| airspeed_doublet = [] | ||
| # wing_rot_doublet = [] | ||
|
|
||
| # Perform analysis | ||
| for i in range(0, len(t_change_doublet), 2): | ||
| t_start = t_change_doublet[i] | ||
| t_end = t_change_doublet[i+1] | ||
| doublet_actuator.append(doublet_axis[change_doublet_idx[i]]) | ||
| # Perform analysis up to 2 seconds after doublet ends | ||
| # Search for index just before the doublet input | ||
| start_idx = np.argmax(t_indi > t_start) - 1 | ||
| end_idx = np.argmax(t_indi > (t_end + 4.)) | ||
|
|
||
| A_sliced = A[start_idx:end_idx + 1] | ||
| dd_rates_sliced = dd_rates[start_idx:end_idx + 1] | ||
|
|
||
| g1_lstsq = np.linalg.lstsq(A_sliced,dd_rates_sliced, rcond=None) | ||
| g1_matrix = g1_lstsq[0] | ||
| g1_residuals = g1_lstsq[1] | ||
|
|
||
| eff_roll.append(g1_matrix[doublet_actuator[-1]][0]) | ||
| eff_pitch.append(g1_matrix[doublet_actuator[-1]][1]) | ||
| eff_yaw.append(g1_matrix[doublet_actuator[-1]][2]) | ||
| eff_acc_x.append(g1_matrix[doublet_actuator[-1]][3]) | ||
| eff_acc_z.append(g1_matrix[doublet_actuator[-1]][4]) | ||
|
|
||
| start_idx_airspeed = np.argmax(t_airdata> t_start) - 1 | ||
| end_idx_airspeed = np.argmax(t_airdata > (t_end + 2.)) | ||
| airspeed_filtered_sliced = airspeed_filtered[start_idx_airspeed:end_idx_airspeed + 1] | ||
| avg_airspeed = sum(airspeed_filtered_sliced) / len(airspeed_filtered_sliced) | ||
| airspeed_doublet.append(avg_airspeed) | ||
|
|
||
| # start_idx_wing_rot = np.argmax(t_rot_wing_controller > t_start) - 1 | ||
| # end_idx_wing_rot = np.argmax(t_rot_wing_controller > (t_end + 2.)) | ||
| # wing_rot_sliced = wing_rot[start_idx_wing_rot:end_idx_wing_rot] | ||
| # avg_wing_angle = sum(wing_rot_sliced) / len(wing_rot_sliced) | ||
| # wing_rot_doublet.append(avg_wing_angle) | ||
|
|
||
| # export data to csv file | ||
| # dict_data = {'idx': doublet_actuator, 'airspeed': airspeed_doublet, 'wing_angle' : wing_rot_doublet, 'roll_eff': eff_roll, 'pitch_eff': eff_pitch, 'yaw_eff': eff_yaw} | ||
| dict_data = {'idx': doublet_actuator, 'airspeed': airspeed_doublet, 'roll_eff': eff_roll, 'pitch_eff': eff_pitch, 'yaw_eff': eff_yaw} | ||
| df = pd.DataFrame(dict_data) | ||
| df.to_csv('test_doublet.csv') | ||
|
|
||
| if plot: | ||
| # Get indices of actuators | ||
| idx0 = np.where(np.array(doublet_actuator) == 0)[0] | ||
| idx1 = np.where(np.array(doublet_actuator) == 1)[0] | ||
| idx2 = np.where(np.array(doublet_actuator) == 2)[0] | ||
| idx3 = np.where(np.array(doublet_actuator) == 3)[0] | ||
| idx5 = np.where(np.array(doublet_actuator) == 5)[0] | ||
| idx6 = np.where(np.array(doublet_actuator) == 6)[0] | ||
|
|
||
| plt.figure('Roll') | ||
| plt.xlabel('doublet id') | ||
| plt.ylabel('pprz_eff') | ||
| plt.ylim(-0.02, 0.02) | ||
| plt.grid() | ||
| plt.scatter(idx0, np.array(eff_roll)[idx0], label="front") | ||
| plt.scatter(idx1, np.array(eff_roll)[idx1], label="right") | ||
| plt.scatter(idx2, np.array(eff_roll)[idx2], label="back") | ||
| plt.scatter(idx3, np.array(eff_roll)[idx3], label="left") | ||
| plt.scatter(idx6, np.array(eff_roll)[idx6], label="push") | ||
| plt.legend() | ||
|
|
||
| plt.figure('Pitch') | ||
| plt.xlabel('doublet id') | ||
| plt.ylabel('pprz_eff') | ||
| plt.ylim(-0.0025, 0.0025) | ||
| plt.grid() | ||
| plt.scatter(idx0, np.array(eff_pitch)[idx0], label="front") | ||
| plt.scatter(idx1, np.array(eff_pitch)[idx1], label="right") | ||
| plt.scatter(idx2, np.array(eff_pitch)[idx2], label="back") | ||
| plt.scatter(idx3, np.array(eff_pitch)[idx3], label="left") | ||
| plt.scatter(idx5, np.array(eff_pitch)[idx5], label="elevator") | ||
| plt.scatter(idx6, np.array(eff_pitch)[idx6], label="push") | ||
| plt.legend() | ||
|
|
||
| plt.figure('Yaw') | ||
| plt.xlabel('doublet id') | ||
| plt.ylabel('pprz_eff') | ||
| plt.ylim(-0.001, 0.001) | ||
| plt.grid() | ||
| plt.scatter(idx0, np.array(eff_yaw)[idx0], label="front") | ||
| plt.scatter(idx1, np.array(eff_yaw)[idx1], label="right") | ||
| plt.scatter(idx2, np.array(eff_yaw)[idx2], label="back") | ||
| plt.scatter(idx3, np.array(eff_yaw)[idx3], label="left") | ||
| plt.scatter(idx6, np.array(eff_roll)[idx6], label="push") | ||
| plt.legend() | ||
|
|
||
| plt.figure('acc_z') | ||
| plt.xlabel('doublet id') | ||
| plt.ylabel('pprz_eff') | ||
| #plt.ylim(-0.001, 0.001) | ||
| plt.grid() | ||
| plt.scatter(idx0, np.array(eff_acc_z)[idx0], label="front") | ||
| plt.scatter(idx1, np.array(eff_acc_z)[idx1], label="right") | ||
| plt.scatter(idx2, np.array(eff_acc_z)[idx2], label="back") | ||
| plt.scatter(idx3, np.array(eff_acc_z)[idx3], label="left") | ||
| plt.scatter(idx6, np.array(eff_acc_z)[idx6], label="push") | ||
| plt.legend() | ||
|
|
||
| # Fit roll effectiveness on airspeed | ||
| A_roll_as_idx0 = np.append([np.array(airspeed_doublet)[idx0]], [np.ones(len(np.array(airspeed_doublet)[idx0]))], axis = 0).T | ||
| A_roll_as_idx1 = np.append([np.array(airspeed_doublet)[idx1]], [np.ones(len(np.array(airspeed_doublet)[idx1]))], axis = 0).T | ||
| A_roll_as_idx2 = np.append([np.array(airspeed_doublet)[idx2]], [np.ones(len(np.array(airspeed_doublet)[idx2]))], axis = 0).T | ||
| A_roll_as_idx3 = np.append([np.array(airspeed_doublet)[idx3]], [np.ones(len(np.array(airspeed_doublet)[idx3]))], axis = 0).T | ||
|
|
||
| roll_coef_as_idx0 = np.linalg.lstsq(A_roll_as_idx0, np.array(eff_roll)[idx0], rcond=-1)[0] | ||
| A_roll_as_idx2 = np.append([np.array(airspeed_doublet)[idx1]], [np.ones(len(np.array(airspeed_doublet)[idx1]))], axis = 0).T | ||
| roll_coef_as_idx1 = np.linalg.lstsq(A_roll_as_idx1, np.array(eff_roll)[idx1], rcond=-1)[0] | ||
| A_roll_as_idx2 = np.append([np.array(airspeed_doublet)[idx2]], [np.ones(len(np.array(airspeed_doublet)[idx2]))], axis = 0).T | ||
| roll_coef_as_idx2 = np.linalg.lstsq(A_roll_as_idx2, np.array(eff_roll)[idx2], rcond=-1)[0] | ||
| A_roll_as_idx3 = np.append([np.array(airspeed_doublet)[idx3]], [np.ones(len(np.array(airspeed_doublet)[idx3]))], axis = 0).T | ||
| roll_coef_as_idx3 = np.linalg.lstsq(A_roll_as_idx3, np.array(eff_roll)[idx3], rcond=-1)[0] | ||
|
|
||
|
|
||
| plt.figure('roll eff vs airspeed') | ||
| plt.xlabel('airspeed [m/s]') | ||
| plt.ylabel('pprz effectiveness') | ||
| plt.grid() | ||
| plt.scatter(np.array(airspeed_doublet)[idx0], np.array(eff_roll)[idx0], label="front") | ||
| plt.scatter(np.array(airspeed_doublet)[idx1], np.array(eff_roll)[idx1], label="right") | ||
| plt.scatter(np.array(airspeed_doublet)[idx2], np.array(eff_roll)[idx2], label="back") | ||
| plt.scatter(np.array(airspeed_doublet)[idx3], np.array(eff_roll)[idx3], label="left") | ||
| plt.plot([0, 6], [roll_coef_as_idx3[1], roll_coef_as_idx3[1] + 6*roll_coef_as_idx3[0]]) | ||
| plt.legend() | ||
| plt.figure('pitch eff vs airspeed') | ||
| plt.xlabel('airspeed [m/s]') | ||
| plt.ylabel('pprz effectiveness') | ||
| plt.grid() | ||
| plt.scatter(np.array(airspeed_doublet)[idx0], np.array(eff_pitch)[idx0], label="front") | ||
| plt.scatter(np.array(airspeed_doublet)[idx1], np.array(eff_pitch)[idx1], label="right") | ||
| plt.scatter(np.array(airspeed_doublet)[idx2], np.array(eff_pitch)[idx2], label="back") | ||
| plt.scatter(np.array(airspeed_doublet)[idx3], np.array(eff_pitch)[idx3], label="left") | ||
| plt.scatter(np.array(airspeed_doublet)[idx5], np.array(eff_pitch)[idx5], label="elevator") | ||
| plt.legend() | ||
|
|
||
| plt.figure('airspeed') | ||
| plt.xlabel('t') | ||
| plt.ylabel('airspeed [m/s]') | ||
| plt.grid() | ||
| plt.plot(t_airdata, airspeed) | ||
| plt.plot(t_airdata, airspeed_filtered) | ||
| plt.show() |