| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
| 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,393 @@ | ||
| /* | ||
| * 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, | ||
| &wing_rotation.wing_angle_deg, | ||
| &wing_rotation.wing_angle_deg_sp, | ||
| &wing_rotation.adc_wing_rotation, | ||
| &wing_rotation.servo_pprz_cmd); | ||
| } | ||
| #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 "firmwares/rotorcraft/guidance/guidance_h.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 | ||
|
|
||
| #if USE_NPS | ||
| #include "modules/actuators/actuators.h" | ||
| #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); | ||
|
|
||
| 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.airspeed_scheduling_nav = false; | ||
| wing_rotation.force_rotation_angle = 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; | ||
| } | ||
|
|
||
| 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); | ||
| } else if (guidance_h.mode == GUIDANCE_H_MODE_NAV) { | ||
| set_wing_rotation_scheduler(wing_rotation.airspeed_scheduling_nav); | ||
| } | ||
|
|
||
| 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; | ||
| if (!wing_rotation.force_rotation_angle) { | ||
| wing_rotation.wing_angle_deg_sp = 0; | ||
| } | ||
| } | ||
| return false; | ||
| } | ||
|
|
||
| bool set_wing_rotation_scheduler_nav(bool rotation_scheduler_on) | ||
| { | ||
| wing_rotation.airspeed_scheduling_nav = rotation_scheduler_on; | ||
| return false; | ||
| } |
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,66 @@ | ||
| /* | ||
| * 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); | ||
| extern bool set_wing_rotation_scheduler_nav(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 airspeed_scheduling_nav; | ||
| bool force_rotation_angle; | ||
| 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 |