diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml index ca2e691818..5641c316fc 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml @@ -139,7 +139,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml index ad4294202a..d72c7d910b 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack.xml @@ -140,7 +140,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml index 48c7a223c5..d11d7fb55f 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml @@ -132,7 +132,7 @@ - + diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml index 2d884eb6b5..51b703baa7 100644 --- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml +++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_simulation.xml @@ -103,7 +103,7 @@ - + diff --git a/conf/modules/eff_scheduling_rot_wing_V2.xml b/conf/modules/eff_scheduling_rot_wing_V2.xml new file mode 100644 index 0000000000..03f1accc04 --- /dev/null +++ b/conf/modules/eff_scheduling_rot_wing_V2.xml @@ -0,0 +1,78 @@ + + + + T + The control effectiveness scheduler for the rotating wing quadplane drone type + - it requires a servo called ROTATION_MECH + +
+ + + + + + + + + + +
+ + + + + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/conf/modules/rotwing_state_V2.xml b/conf/modules/rotwing_state_V2.xml new file mode 100644 index 0000000000..8da136c51d --- /dev/null +++ b/conf/modules/rotwing_state_V2.xml @@ -0,0 +1,29 @@ + + + + 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. +
+ +
+ + + + + + + + + + + + +
+ +
+ + + + + + + diff --git a/conf/modules/wing_rotation_adc_sensor.xml b/conf/modules/wing_rotation_adc_sensor.xml index 70161cd08c..71f7438c8d 100644 --- a/conf/modules/wing_rotation_adc_sensor.xml +++ b/conf/modules/wing_rotation_adc_sensor.xml @@ -13,7 +13,7 @@ - + diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.c b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.c new file mode 100644 index 0000000000..6e85fe7bbd --- /dev/null +++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.c @@ -0,0 +1,402 @@ +/* + * Copyright (C) 2023 Tomaso De Ponti + * + * 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 + * . + */ + +/** @file "modules/ctrl/eff_scheduling_rot_wing_V2.c" + * @author Tomaso De Ponti + * The control effectiveness scheduler for the rotating wing drone type + */ + +#include "modules/ctrl/eff_scheduling_rot_wing_V2.h" +#include "generated/airframe.h" +#include "state.h" +#include "modules/actuators/actuators.h" +#include "modules/core/abi.h" +#include "filters/low_pass_filter.h" + +#define FORCE_ONELOOP +#ifdef FORCE_ONELOOP +#include "firmwares/rotorcraft/oneloop/oneloop_andi.h" +float actuator_state_filt_vect[EFF_MAT_COLS_NB] = {0}; +#else +#include "firmwares/rotorcraft/stabilization/stabilization_indi.h" +#endif + +#ifndef ROT_WING_EFF_SCHED_IXX_BODY +#error "NO ROT_WING_EFF_SCHED_IXX_BODY defined" +#endif + +#ifndef ROT_WING_EFF_SCHED_IYY_BODY +#error "NO ROT_WING_EFF_SCHED_IYY_BODY defined" +#endif + +#ifndef ROT_WING_EFF_SCHED_IZZ +#error "NO ROT_WING_EFF_SCHED_IZZ defined" +#endif + +#ifndef ROT_WING_EFF_SCHED_IXX_WING +#error "NO ROT_WING_EFF_SCHED_IXX_WING defined" +#endif + +#ifndef ROT_WING_EFF_SCHED_IYY_WING +#error "NO ROT_WING_EFF_SCHED_IYY_WING defined" +#endif + +#ifndef ROT_WING_EFF_SCHED_M +#error "NO ROT_WING_EFF_SCHED_M defined" +#endif + +/* Effectiveness Matrix definition */ +float G2_RW[EFF_MAT_COLS_NB] = {0};//ROT_WING_EFF_SCHED_G2; //scaled by RW_G_SCALE +float G1_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB] = {0};//{ROT_WING_EFF_SCHED_G1_ZERO, ROT_WING_EFF_SCHED_G1_ZERO, ROT_WING_EFF_SCHED_G1_THRUST, ROT_WING_EFF_SCHED_G1_ROLL, ROT_WING_EFF_SCHED_G1_PITCH, ROT_WING_EFF_SCHED_G1_YAW}; //scaled by RW_G_SCALE +float EFF_MAT_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB] = {0}; +static float flt_cut = 1.0e-4; + +struct FloatEulers eulers_zxy_RW_EFF; +static Butterworth2LowPass skew_filt; +/* Temp variables*/ +bool airspeed_fake_on = false; +float airspeed_fake = 0.0; +float ele_eff = 22.0; +float ele_min = 0.0; +/* Define Forces and Moments tructs for each actuator*/ +struct RW_Model RW; + +inline void eff_scheduling_rot_wing_update_wing_angle(void); +inline void eff_scheduling_rot_wing_update_airspeed(void); +void ele_pref_sched(void); +void update_attitude(void); +void sum_EFF_MAT_RW(void); +void init_RW_Model(void); +void calc_G1_G2_RW(void); + +/** ABI binding wing position data. + */ +#ifndef WING_ROTATION_CAN_ROT_WING_ID +#define WING_ROTATION_CAN_ROT_WING_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(WING_ROTATION_CAN_ROT_WING_ID) +static abi_event wing_position_ev; + +float skew_meas = 0.0; +static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *pos_msg, uint8_t num_act) +{ + for (int i=0; i ELE_MIN_AS){ + RW.ele_pref = (ZERO_ELE_PPRZ - ele_min) / (ELE_MAX_AS - ELE_MIN_AS) * (RW.as - ELE_MIN_AS) + ele_min; + Bound(RW.ele_pref,ele_min,ZERO_ELE_PPRZ); + } else { + RW.ele_pref = ele_min; + } +} diff --git a/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.h b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.h new file mode 100644 index 0000000000..c47c48817a --- /dev/null +++ b/sw/airborne/modules/ctrl/eff_scheduling_rot_wing_V2.h @@ -0,0 +1,202 @@ +/* + * Copyright (C) 2023 Tomaso De Ponti + * + * 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 + * . + */ + +/** @file "modules/ctrl/eff_scheduling_rot_wing_V2.h" + * @author Tomaso De Ponti + * The control effectiveness scheduler for the rotating wing drone type + */ + +#ifndef CTRL_EFF_SCHED_ROT_WING_H +#define CTRL_EFF_SCHED_ROT_WING_H + +#include "std.h" +#include "generated/airframe.h" + +#ifndef EFF_MAT_ROWS_NB +#define EFF_MAT_ROWS_NB 6 +#endif + +#define RW_aX 0 // X body axis (linear acceleration) +#define RW_aY 1 // Y body axis (linear acceleration) +#define RW_aZ 2 // Z body axis (linear acceleration) +#define RW_aN 0 // North axis (linear acceleration) +#define RW_aE 1 // East axis (linear acceleration) +#define RW_aD 2 // Down axis (linear acceleration) +#define RW_ap 3 // X body axis (angular acceleration) +#define RW_aq 4 // Y body axis (angular acceleration) +#define RW_ar 5 // Z body axis (angular acceleration) + +#ifndef EFF_MAT_COLS_NB +#define EFF_MAT_COLS_NB (COMMANDS_NB_REAL + COMMANDS_NB_VIRTUAL) +#endif + +#define RW_G_SCALE 1000.0f + +// Elevator ctrl definitions +#ifndef ZERO_ELE_PPRZ +#define ZERO_ELE_PPRZ 8000 +#endif + +#ifndef ELE_MIN_AS +#define ELE_MIN_AS 8.8 +#endif + +#ifndef ELE_MAX_AS // should match the max airspeed +#define ELE_MAX_AS 16.0 +#endif + +extern float EFF_MAT_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB]; +extern float G2_RW[EFF_MAT_COLS_NB] ; +extern float G1_RW[EFF_MAT_ROWS_NB][EFF_MAT_COLS_NB] ; +struct rot_wing_eff_sched_param_t { + float Ixx_body; // body MMOI around roll axis [kgm²] + float Iyy_body; // body MMOI around pitch axis [kgm²] + float Izz; // total MMOI around yaw axis [kgm²] + float Ixx_wing; // wing MMOI around the chordwise direction of the wing [kgm²] + float Iyy_wing; // wing MMOI around the spanwise direction of the wing [kgm²] + float m; // mass [kg] + float DMdpprz_hover_roll[2]; // Moment coeficients for roll motors (Scaled by 10000) + float hover_roll_pitch_coef[2]; // Model coefficients to correct pitch effective for roll motors + float hover_roll_roll_coef[2]; // Model coefficients to correct roll effectiveness for roll motors + float k_elevator[3]; + float k_rudder[3]; + float k_aileron; + float k_flaperon; + float k_pusher[2]; + float k_elevator_deflection[2]; + float d_rudder_d_pprz; + float k_rpm_pprz_pusher[3]; + float k_lift_wing[2]; + float k_lift_fuselage; + float k_lift_tail; +}; + +struct rot_wing_eff_sched_var_t { + float Ixx; // Total MMOI around roll axis [kgm²] + float Iyy; // Total MMOI around pitch axis [kgm²] + float wing_rotation_rad; // Wing rotation angle in radians: from ABI message + float wing_rotation_deg; // Wing rotation angle in degrees: (clone in degrees) + float cosr; // cosine of wing rotation angle + float sinr; // sine of wing rotation angle + float cosr2; // cosine² of wing rotation angle + float sinr2; // sine² of wing rotation angle + float cosr3; // cosine³ of wing rotation angle + float sinr3; // sine³ of wing rotation angle + + // Set during initialization + float pitch_motor_dMdpprz; // derivative of delta moment with respect to a delta paparazzi command for the pitch motors [Nm/pprz] + float roll_motor_dMdpprz; // derivative of delta moment with respect to a delta paparazzi command for the roll motors [Nm/pprz] + + // commands + float cmd_elevator; + float cmd_pusher; + float cmd_pusher_scaled; + float cmd_T_mean_scaled; + + // airspeed + float airspeed; + float airspeed2; +}; + +struct I{ + float xx; + float yy; + float zz; + float w_xx; + float w_yy; + float b_xx; + float b_yy; +}; +struct F_M_Body{ + float dFdu; // derivative of the force with respect to the control input (e.g. linear coefficient) + float dMdu; // derivative of the reaction trque with respect to the control input (e.g. linear coefficient) + float dMdud; // derivative of the reaction torque with respect to the control input time derivative + float l; // arm length +}; + +struct wing_model{ + float k0; // Linear coefficient (theta u²) + float k1; // Linear coeeficient (theta u² s(Lambda)²) + float k2; // linear coefficient with (u² s(Lambda)²) + float dLdtheta; // derivative of lift with respect to the pitch angle + float L; // Lift +}; + +struct RW_attitude{ + float phi; + float theta; + float psi; + float sphi; + float cphi; + float stheta; + float ctheta; + float spsi; + float cpsi; +}; + +struct RW_skew{ + float rad; // Wing rotation angle in radians: from ABI message + float deg; // Wing rotation angle in degrees: (clone in degrees) + float cosr; // cosine of wing rotation angle + float sinr; // sine of wing rotation angle + float cosr2; // cosine² of wing rotation angle + float sinr2; // sine² of wing rotation angle + float cosr3; // cosine³ of wing rotation angle + float sinr3; // sine³ of wing rotation angle +}; +struct RW_Model{ + struct I I; // Inertia matrix + float m; // mass [kg] + float T; // Thrust [N] + float P; // Pusher thrust [N] + struct RW_skew skew; + struct RW_attitude att; + struct wing_model wing; + struct F_M_Body mF; + struct F_M_Body mR; + struct F_M_Body mB; + struct F_M_Body mL; + struct F_M_Body mP; + struct F_M_Body ele; + struct F_M_Body rud; + struct F_M_Body ail; + struct F_M_Body flp; + float as; // airspeed [m/s] + float as2; // airspeed squared [m/s²] + float ele_pref; + +}; + +extern bool airspeed_fake_on; +extern float airspeed_fake; +extern float ele_eff; +extern float ele_min; + +extern float rotation_angle_setpoint_deg; +extern int16_t rotation_cmd; + +extern float eff_sched_pusher_time; + +extern void eff_scheduling_rot_wing_init(void); +extern void eff_scheduling_rot_wing_periodic(void); + +extern struct RW_Model RW; +#endif // CTRL_EFF_SCHED_ROT_WING_H + diff --git a/sw/airborne/modules/rot_wing_drone/rot_wing_automation.h b/sw/airborne/modules/rot_wing_drone/rot_wing_automation.h index 2ca119d0b4..d9d2e303b7 100644 --- a/sw/airborne/modules/rot_wing_drone/rot_wing_automation.h +++ b/sw/airborne/modules/rot_wing_drone/rot_wing_automation.h @@ -28,7 +28,12 @@ #include "std.h" #include "math/pprz_algebra_float.h" + +#ifdef RW_USE_MODULES_V2 +#include "modules/rot_wing_drone/rotwing_state_V2.h" +#else #include "modules/rot_wing_drone/rotwing_state.h" +#endif extern void init_rot_wing_automation(void); extern void periodic_rot_wing_automation(void); diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state_V2.c b/sw/airborne/modules/rot_wing_drone/rotwing_state_V2.c new file mode 100644 index 0000000000..46b18c4040 --- /dev/null +++ b/sw/airborne/modules/rot_wing_drone/rotwing_state_V2.c @@ -0,0 +1,773 @@ +/* + * Copyright (C) 2023 Tomaso De Ponti + * + * 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 + * . + */ + +/** @file "modules/rotwing/rotwing_state_V2.c" + * @author Tomaso De Ponti + * 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_V2.h" +#include "firmwares/rotorcraft/autopilot_firmware.h" +#include "modules/core/commands.h" +#include "modules/actuators/actuators.h" +#include "modules/core/abi.h" + +// 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 + +// maximum quad airspeed to force quad state +#ifndef ROTWING_MAX_QUAD_AIRSPEED +#define ROTWING_MAX_QUAD_AIRSPEED 20.0 +#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_RUN_RPM_TH +#define ROTWING_HOV_MOT_RUN_RPM_TH 800 +#endif +#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_USE_ROTATION_REF_MODEL +#define ROTWING_STATE_USE_ROTATION_REF_MODEL FALSE +#endif + +// Hover preferred pitch (deg) +#ifndef ROTWING_STATE_HOVER_PREF_PITCH +#define ROTWING_STATE_HOVER_PREF_PITCH 0.0 +#endif + +// Transition preferred pitch (deg) +#ifndef ROTWING_STATE_TRANSITION_PREF_PITCH +#define ROTWING_STATE_TRANSITION_PREF_PITCH 3.0 +#endif + +// Forward preferred pitch (deg) +#ifndef ROTWING_STATE_FW_PREF_PITCH +#define ROTWING_STATE_FW_PREF_PITCH 8.0 +#endif + +// Make sure the rotmech dynamics are provided if the virtual rotmech is used +#ifndef USE_ROTMECH_VIRTUAL +#define USE_ROTMECH_VIRTUAL FALSE +#endif + +#if USE_ROTMECH_VIRTUAL +#ifndef ROTMECH_DYN +#error "Rotmech dynamics are not provided. Please provide them in your airframe file." +#endif +#endif + +// stream ADC data if ADC rotation sensor +#ifndef ADC_WING_ROTATION +#define ADC_WING_ROTATION FALSE +#endif +#if ADC_WING_ROTATION +#include "wing_rotation_adc_sensor.h" +#endif +/** ABI binding feedback data. + */ +#ifndef ROTWING_STATE_ACT_FEEDBACK_ID +#define ROTWING_STATE_ACT_FEEDBACK_ID ABI_BROADCAST +#endif +abi_event rotwing_state_feedback_ev; +static void rotwing_state_feedback_cb(uint8_t sender_id, struct act_feedback_t *feedback_msg, 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}; + +struct RotwingState rotwing_state; +struct RotWingStateSettings rotwing_state_settings; +struct RotWingStateSkewing rotwing_state_skewing; + +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; + +float rotwing_state_max_hover_speed = 7; +#ifdef NAV_HYBRID_MAX_AIRSPEED +float rotwing_state_max_fw_speed = NAV_HYBRID_MAX_AIRSPEED; +#else +float rotwing_state_max_fw_speed = 20; +#endif + +bool hover_motors_active = true; +bool bool_disable_hover_motors = false; + +inline void rotwing_check_set_current_state(void); +inline void rotwing_switch_state(void); + +inline void rotwing_state_set_hover_settings(void); +inline void rotwing_state_set_skewing_settings(void); +inline void rotwing_state_set_fw_settings(void); +inline void rotwing_state_set_fw_hov_mot_idle_settings(void); +inline void rotwing_state_set_fw_hov_mot_off_settings(void); + +inline void rotwing_state_set_state_settings(void); +inline void rotwing_state_skewer(void); +inline void rotwing_state_free_processor(void); + +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" +static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev) +{ + uint16_t adc_dummy = 0; + + pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID, + &rotwing_state.current_state, + &rotwing_state.desired_state, + &rotwing_state_skewing.wing_angle_deg, + &rotwing_state_skewing.wing_angle_deg_sp, + &adc_dummy, + &rotwing_state_skewing.servo_pprz_cmd); +} +#endif // PERIODIC_TELEMETRY + +void rotwing_state_force_skew_off(void) +{ + rotwing_state_skewing.force_rotation_angle = false; +} + +void init_rotwing_state(void) +{ + // Bind ABI messages + AbiBindMsgACT_FEEDBACK(ROTWING_STATE_ACT_FEEDBACK_ID, &rotwing_state_feedback_ev, rotwing_state_feedback_cb); + + // Start the drone in a desired hover state + rotwing_state.current_state = ROTWING_STATE_HOVER; + rotwing_state.desired_state = ROTWING_STATE_HOVER; + rotwing_state.requested_config = ROTWING_CONFIGURATION_HOVER; + + rotwing_state_settings.preferred_pitch_value = 0; + + rotwing_state_skewing.wing_angle_deg_sp = 0; + rotwing_state_skewing.wing_angle_deg = 0; + rotwing_state_skewing.servo_pprz_cmd = -MAX_PPRZ; + rotwing_state_skewing.airspeed_scheduling = false; + rotwing_state_skewing.force_rotation_angle = false; + +#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 + if (guidance_h.mode == GUIDANCE_H_MODE_NAV) { + // Run the free state requester if free configuration requestes + if(rotwing_state.requested_config == ROTWING_CONFIGURATION_FREE) { + rotwing_state_free_processor(); + } + rotwing_switch_state(); + } else if (guidance_h.mode == GUIDANCE_H_MODE_NONE) { + if (stabilization.mode == STABILIZATION_MODE_ATTITUDE) { + if (stabilization.att_submode == STABILIZATION_ATT_SUBMODE_FORWARD) { + rotwing_state_set_fw_settings(); + } else { + rotwing_state_set_hover_settings(); + } + } + } + // } else { + // rotwing_switch_state(); + // } + + // Run the wing skewer + rotwing_state_skewer(); + + //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_NONE) { + bool_disable_hover_motors = false; + } + struct FloatEulers eulers_zxy; + float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); + + // Evaluate motors_on boolean + if (!hover_motors_active) { + if (stateGetAirspeed_f() < 15.) { + hover_motors_active = true; + bool_disable_hover_motors = false; + } else if (eulers_zxy.theta > RadOfDeg(15.0)) { + hover_motors_active = true; + bool_disable_hover_motors = false; + } + } else { + bool_disable_hover_motors = false; + } + float pitch_progression = (rotwing_state_skewing.wing_angle_deg - ROTWING_HALF_SKEW_ANGLE_DEG) / (90.0-ROTWING_HALF_SKEW_ANGLE_DEG); + // Calculate scheduled pitch angle + switch (rotwing_state_settings.preferred_pitch_setting){ + case ROTWING_STATE_PITCH_QUAD_SETTING: + rotwing_state_settings.preferred_pitch_value = RadOfDeg(ROTWING_STATE_HOVER_PREF_PITCH); + break; + case ROTWING_STATE_PITCH_TRANSITION_SETTING: + rotwing_state_settings.preferred_pitch_value = RadOfDeg(ROTWING_STATE_TRANSITION_PREF_PITCH * pitch_progression); + break; + case ROTWING_STATE_PITCH_FW_SETTING: + rotwing_state_settings.preferred_pitch_value = RadOfDeg(ROTWING_STATE_FW_PREF_PITCH); + break; + } +} + +// Function to request a state +void request_rotwing_state(uint8_t state) +{ + if (state <= ROTWING_STATE_FW_HOV_MOT_OFF) { + rotwing_state.desired_state = state; + } +} + +// Function to prefer configuration +void rotwing_request_configuration(uint8_t configuration) +{ + switch (configuration) { + case ROTWING_CONFIGURATION_HOVER: + request_rotwing_state(ROTWING_STATE_HOVER); + rotwing_state.requested_config = ROTWING_CONFIGURATION_HOVER; + break; + case ROTWING_CONFIGURATION_HYBRID: + request_rotwing_state(ROTWING_STATE_SKEWING); + rotwing_state.requested_config = ROTWING_CONFIGURATION_HYBRID; + break; + case ROTWING_CONFIGURATION_EFFICIENT: + request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF); + rotwing_state.requested_config = ROTWING_CONFIGURATION_EFFICIENT; + break; + case ROTWING_CONFIGURATION_FREE: + rotwing_state.requested_config = ROTWING_CONFIGURATION_FREE; + break; + } +} + +void rotwing_check_set_current_state(void) +{ + float current_thrust = (commands[COMMAND_MOTOR_FRONT] + commands[COMMAND_MOTOR_RIGHT] + commands[COMMAND_MOTOR_BACK] + commands[COMMAND_MOTOR_LEFT])/4.0; + // 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 (rotwing_state_skewing.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 (rotwing_state_skewing.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 (rotwing_state_skewing.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 (rotwing_state_skewing.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 (current_thrust < ROTWING_MIN_THRUST_IDLE && rotwing_state.desired_state > ROTWING_STATE_FW) { + 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 (current_thrust > ROTWING_MIN_THRUST_IDLE + || rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_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) { +#if !USE_NPS + rotwing_state_fw_m_off_counter++; +#endif + } 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_idle_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; + } +} + +// 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) { + rotwing_state_set_skewing_settings(); + } else { + rotwing_state_set_hover_settings(); + } + break; + + case ROTWING_STATE_SKEWING: + if (rotwing_state.desired_state < ROTWING_STATE_SKEWING && stateGetAirspeed_f() < ROTWING_MAX_QUAD_AIRSPEED) { + rotwing_state_set_hover_settings(); + } else { + rotwing_state_set_skewing_settings(); + } + break; + + case ROTWING_STATE_FW: + if (rotwing_state.desired_state > ROTWING_STATE_FW) { + rotwing_state_set_fw_hov_mot_idle_settings(); + } else if (rotwing_state.desired_state < ROTWING_STATE_FW) { + rotwing_state_set_skewing_settings(); + } else { + rotwing_state_set_fw_settings(); + } + break; + + case ROTWING_STATE_FW_HOV_MOT_IDLE: + if (rotwing_state.desired_state > ROTWING_STATE_FW_HOV_MOT_IDLE) { + rotwing_state_set_fw_hov_mot_off_settings(); + } else if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_IDLE) { + rotwing_state_set_fw_settings(); + } else { + rotwing_state_set_fw_hov_mot_idle_settings(); + } + break; + + case ROTWING_STATE_FW_HOV_MOT_OFF: + if (rotwing_state.desired_state < ROTWING_STATE_FW_HOV_MOT_OFF) { + rotwing_state_set_fw_hov_mot_idle_settings(); + } else { + rotwing_state_set_fw_hov_mot_off_settings(); + } + break; + } +} + +void rotwing_state_set_hover_settings(void) +{ + rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_QUAD_SETTING; + rotwing_state_settings.hover_motors_active = true; + rotwing_state_settings.hover_motors_disable = false; + rotwing_state_settings.force_forward = false; + rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_QUAD_SETTING; + rotwing_state_settings.stall_protection = false; + rotwing_state_settings.max_v_climb = 2.0; + rotwing_state_settings.max_v_descend = 1.0; + rotwing_state_settings.nav_max_speed = rotwing_state_max_hover_speed; // Using setting + rotwing_state_set_state_settings(); +} + +void rotwing_state_set_skewing_settings(void) +{ + // Wing may be skewed to quad when desired state is hover and skewing settings set by state machine + if (rotwing_state.desired_state == ROTWING_STATE_HOVER) { + rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_QUAD_SETTING; + } else { + rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_SCHEDULING_SETTING; + } + rotwing_state_settings.hover_motors_active = true; + rotwing_state_settings.hover_motors_disable = false; + rotwing_state_settings.force_forward = false; + rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_TRANSITION_SETTING; + rotwing_state_settings.stall_protection = false; + rotwing_state_settings.max_v_climb = 2.0; + rotwing_state_settings.max_v_descend = 1.0; + rotwing_state_settings.nav_max_speed = rotwing_state_max_fw_speed;// Big as we use airspeed guidance + rotwing_state_set_state_settings(); +} + +void rotwing_state_set_fw_settings(void) +{ + rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_FW_SETTING; + rotwing_state_settings.hover_motors_active = true; + rotwing_state_settings.hover_motors_disable = false; + rotwing_state_settings.force_forward = true; + rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_FW_SETTING; + rotwing_state_settings.stall_protection = true; + rotwing_state_settings.max_v_climb = 4.0; + rotwing_state_settings.max_v_descend = 4.0; + rotwing_state_settings.nav_max_speed = rotwing_state_max_fw_speed; // Big as we use airspeed guidance + //rotwing_state_settings.nav_max_speed = 100; // Big as we use airspeed guidance + rotwing_state_set_state_settings(); +} + +void rotwing_state_set_fw_hov_mot_idle_settings(void) +{ + rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_FW_SETTING; + rotwing_state_settings.hover_motors_active = false; + rotwing_state_settings.hover_motors_disable = false; + rotwing_state_settings.force_forward = true; + rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_FW_SETTING; + rotwing_state_settings.stall_protection = true; + rotwing_state_settings.max_v_climb = 4.0; + rotwing_state_settings.max_v_descend = 4.0; + rotwing_state_settings.nav_max_speed = rotwing_state_max_fw_speed; // Big as we use airspeed guidance + rotwing_state_set_state_settings(); +} + +void rotwing_state_set_fw_hov_mot_off_settings(void) +{ + rotwing_state_settings.wing_scheduler = ROTWING_STATE_WING_FW_SETTING; + rotwing_state_settings.hover_motors_active = false; + rotwing_state_settings.hover_motors_disable = true; + rotwing_state_settings.force_forward = true; + rotwing_state_settings.preferred_pitch_setting = ROTWING_STATE_PITCH_FW_SETTING; + rotwing_state_settings.stall_protection = true; + rotwing_state_settings.max_v_climb = 4.0; + rotwing_state_settings.max_v_descend = 4.0; + rotwing_state_settings.nav_max_speed = rotwing_state_max_fw_speed; // Big as we use airspeed guidance + rotwing_state_set_state_settings(); +} + +void rotwing_state_set_state_settings(void) +{ + if (!rotwing_state_skewing.force_rotation_angle) { + switch (rotwing_state_settings.wing_scheduler) { + case ROTWING_STATE_WING_QUAD_SETTING: + rotwing_state_skewing.airspeed_scheduling = false; + rotwing_state_skewing.wing_angle_deg_sp = 0; + break; + case ROTWING_STATE_WING_SCHEDULING_SETTING: + rotwing_state_skewing.airspeed_scheduling = true; + break; + case ROTWING_STATE_WING_FW_SETTING: + rotwing_state_skewing.airspeed_scheduling = true; + break; + } + } else { + rotwing_state_skewing.airspeed_scheduling = false; + } + + hover_motors_active = rotwing_state_settings.hover_motors_active; + + bool_disable_hover_motors = rotwing_state_settings.hover_motors_disable; + + force_forward = rotwing_state_settings.force_forward; + + nav_max_speed = rotwing_state_settings.nav_max_speed; + nav_goto_max_speed = rotwing_state_settings.nav_max_speed; + + // TO DO: pitch angle now hard coded scheduled by wing angle + + // Stall protection handled by hover_motors_active boolean + + // TO DO: Climb and descend speeds now handled by guidance airspeed +} + +void rotwing_state_skewer(void) +{ + if (rotwing_state_skewing.airspeed_scheduling) { + float wing_angle_scheduled_sp_deg = 0; + float airspeed = stateGetAirspeed_f(); + if (airspeed < 8) { + wing_angle_scheduled_sp_deg = 0; + } else if (airspeed < 10 && (rotwing_state.desired_state > ROTWING_STATE_HOVER)) { + wing_angle_scheduled_sp_deg = ROTWING_HALF_SKEW_ANGLE_DEG; + } else if (airspeed > 10) { + wing_angle_scheduled_sp_deg = ((airspeed - 10.)) / 4. * (90.-ROTWING_HALF_SKEW_ANGLE_DEG) + ROTWING_HALF_SKEW_ANGLE_DEG; + } else { + wing_angle_scheduled_sp_deg = 0; + } + + Bound(wing_angle_scheduled_sp_deg, 0., 90.) + rotwing_state_skewing.wing_angle_deg_sp = wing_angle_scheduled_sp_deg; + } +} + +void rotwing_state_free_processor(void) +{ + // Get current speed vector + struct EnuCoor_f *groundspeed = stateGetSpeedEnu_f(); + float current_groundspeed = FLOAT_VECT2_NORM(*groundspeed); + + // Get current airspeed + float airspeed = stateGetAirspeed_f(); + + // Get windspeed vector + struct FloatEulers eulers_zxy; + float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f()); + + float psi = eulers_zxy.psi; + float cpsi = cosf(psi); + float spsi = sinf(psi); + struct FloatVect2 airspeed_v = { spsi * airspeed, cpsi * airspeed }; + struct FloatVect2 windspeed_v; + VECT2_DIFF(windspeed_v, *groundspeed, airspeed_v); + + // Get goto target information + struct FloatVect2 pos_error; + struct EnuCoor_f *pos = stateGetPositionEnu_f(); + VECT2_DIFF(pos_error, nav_rotorcraft_base.goto_wp.to, *pos); + + /* + Calculations + */ + // speed over pos_error projection + struct FloatVect2 pos_error_norm; + VECT2_COPY(pos_error_norm, pos_error); + float_vect2_normalize(&pos_error_norm); + float dist_to_target = sqrtf(nav_rotorcraft_base.goto_wp.dist2_to_wp); + float max_speed_decel2 = fabsf(2.f * dist_to_target * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case + float max_speed_decel = sqrtf(max_speed_decel2); + + // Check if speed setpoint above set airspeed + struct FloatVect2 desired_airspeed_v; + struct FloatVect2 groundspeed_sp; + groundspeed_sp.x = pos_error.x * nav_hybrid_pos_gain; + groundspeed_sp.y = pos_error.y * nav_hybrid_pos_gain; + VECT2_COPY(desired_airspeed_v, groundspeed_sp); + VECT2_ADD(desired_airspeed_v, windspeed_v); + + float desired_airspeed = FLOAT_VECT2_NORM(desired_airspeed_v); + float airspeed_error = rotwing_state_max_fw_speed - airspeed; + + // Request hybrid if we have to decelerate and approaching target + if (max_speed_decel < current_groundspeed) { + request_rotwing_state(ROTWING_STATE_SKEWING); + } else if ((desired_airspeed > 15) && ((current_groundspeed + airspeed_error) < max_speed_decel)) { + request_rotwing_state(ROTWING_STATE_FW_HOV_MOT_OFF); + } +} + +void rotwing_state_skew_actuator_periodic(void) +{ + + // calc rotation percentage of setpoint (0 deg = -1, 45 deg = 0, 90 deg = 1) + float wing_rotation_percentage = (rotwing_state_skewing.wing_angle_deg_sp - 45.) / 45.; + Bound(wing_rotation_percentage, -1., 1.); + + float servo_pprz_cmd = MAX_PPRZ * wing_rotation_percentage; + // Calulcate rotation_cmd + Bound(servo_pprz_cmd, -MAX_PPRZ, MAX_PPRZ); + +#if ROTWING_STATE_USE_ROTATION_REF_MODEL + // Rotate with second order filter + static float rotwing_state_skew_p_cmd = -MAX_PPRZ; + static float rotwing_state_skew_d_cmd = 0; + float speed_sp = 0.001 * (servo_pprz_cmd - rotwing_state_skew_p_cmd); + rotwing_state_skew_d_cmd += 0.003 * (speed_sp - rotwing_state_skew_d_cmd); + rotwing_state_skew_p_cmd += rotwing_state_skew_d_cmd; + Bound(rotwing_state_skew_p_cmd, -MAX_PPRZ, MAX_PPRZ); + rotwing_state_skewing.servo_pprz_cmd = rotwing_state_skew_p_cmd; +#else + // Directly controlling the wing rotation + rotwing_state_skewing.servo_pprz_cmd = servo_pprz_cmd; +#endif + +#if USE_NPS + // Export to the index of the SKEW in the NPS_ACTUATOR_NAMES array + commands[COMMAND_ROT_MECH] = (rotwing_state_skewing.servo_pprz_cmd + MAX_PPRZ) / 2.; // Scale to simulation command + +#if USE_ROTMECH_VIRTUAL + // Calculate discrete first order dynamics of rot mech + float dyn_dis = 1.0-exp(-ROTMECH_DYN / PERIODIC_FREQUENCY); + // Simulate wing angle from command + float prev_rotmech_state = rotwing_state_skewing.wing_angle_deg; + rotwing_state_skewing.wing_angle_deg = prev_rotmech_state + dyn_dis * (rotwing_state_skewing.wing_angle_deg_sp - prev_rotmech_state); +#else + // Simulate wing angle from command + rotwing_state_skewing.wing_angle_deg = (float) rotwing_state_skewing.servo_pprz_cmd / MAX_PPRZ * 45. + 45.; +#endif // USE_ROTMECH_VIRTUAL + // SEND ABI Message to ctr_eff_sched and other modules that want Actuator position feedback + struct act_feedback_t feedback; + feedback.idx = COMMAND_ROT_MECH; + feedback.position = 0.5 * M_PI - RadOfDeg(rotwing_state_skewing.wing_angle_deg); + feedback.set.position = true; + // Send ABI message + AbiSendMsgACT_FEEDBACK(ACT_FEEDBACK_UAVCAN_ID, &feedback, 1); +#else + commands[COMMAND_ROT_MECH] = rotwing_state_skewing.servo_pprz_cmd; +#endif // USE_NPS +} + +static void rotwing_state_feedback_cb(uint8_t __attribute__((unused)) sender_id, + struct act_feedback_t UNUSED *feedback_msg, uint8_t UNUSED num_act_message) +{ + for (int i = 0; i < num_act_message; i++) { + + for (int i = 0; i < num_act_message; i++) { + // Check for wing rotation feedback + if ((feedback_msg[i].set.position) && (feedback_msg[i].idx == COMMAND_ROT_MECH)) { + // Get wing rotation angle from sensor + float wing_angle_rad = 0.5 * M_PI - feedback_msg[i].position; + rotwing_state_skewing.wing_angle_deg = DegOfRad(wing_angle_rad); + + // Bound wing rotation angle + Bound(rotwing_state_skewing.wing_angle_deg, 0, 90.); + } + } + + // Sanity check that index is valid + int idx = feedback_msg[i].idx; + if (feedback_msg[i].set.rpm) { + if ((idx == SERVO_MOTOR_FRONT_IDX) || (idx == SERVO_BMOTOR_FRONT_IDX)) { + rotwing_state_hover_rpm[0] = feedback_msg->rpm; + } else if ((idx == SERVO_MOTOR_RIGHT_IDX) || (idx == SERVO_BMOTOR_RIGHT_IDX)) { + rotwing_state_hover_rpm[1] = feedback_msg->rpm; + } else if ((idx == SERVO_MOTOR_BACK_IDX) || (idx == SERVO_BMOTOR_BACK_IDX)) { + rotwing_state_hover_rpm[2] = feedback_msg->rpm; + } else if ((idx == SERVO_MOTOR_LEFT_IDX) || (idx == SERVO_BMOTOR_LEFT_IDX)) { + rotwing_state_hover_rpm[3] = feedback_msg->rpm; + } + } + } +} + diff --git a/sw/airborne/modules/rot_wing_drone/rotwing_state_V2.h b/sw/airborne/modules/rot_wing_drone/rotwing_state_V2.h new file mode 100644 index 0000000000..d3b4068be0 --- /dev/null +++ b/sw/airborne/modules/rot_wing_drone/rotwing_state_V2.h @@ -0,0 +1,98 @@ +/* + * Copyright (C) 2023 Tomaso De Ponti + * + * 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 + * . + */ + +/** @file "modules/rotwing/rotwing_state_V2.h" + * @author Tomaso De Ponti + * 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 + +/** Rotwing Configurations + */ +#define ROTWING_CONFIGURATION_HOVER 0 // UAV is in hover +#define ROTWING_CONFIGURATION_HYBRID 1 // UAV can fly forward and hover if airspeed low, hover motors on +#define ROTWING_CONFIGURATION_EFFICIENT 2 // UAV flies efficiently forward (FW) +#define ROTWING_CONFIGURATION_FREE 3 // UAV switched between efficient and hybrid dependent on deceleration + +struct RotwingState { + uint8_t current_state; + uint8_t desired_state; + uint8_t requested_config; +}; + +#define ROTWING_STATE_WING_QUAD_SETTING 0 // Wing skew at 0 degrees +#define ROTWING_STATE_WING_SCHEDULING_SETTING 1 // Wing skew handled by airspeed scheduler +#define ROTWING_STATE_WING_FW_SETTING 2 // Wing skew at 90 degrees + +#define ROTWING_STATE_PITCH_QUAD_SETTING 0 // Pitch at prefered hover +#define ROTWING_STATE_PITCH_TRANSITION_SETTING 1 // Pitch scheduled +#define ROTWING_STATE_PITCH_FW_SETTING 2 // Pitch at prefered forward + +struct RotWingStateSettings { + uint8_t wing_scheduler; + bool hover_motors_active; + bool hover_motors_disable; + bool force_forward; + uint8_t preferred_pitch_setting; + float preferred_pitch_value; + bool stall_protection; + float max_v_climb; + float max_v_descend; + float nav_max_speed; +}; + +struct RotWingStateSkewing { + float wing_angle_deg_sp; // Wing angle setpoint in deg + float wing_angle_deg; // Wing angle from sensor in deg + int32_t servo_pprz_cmd; // Wing rotation servo pprz cmd + bool airspeed_scheduling; // Airspeed scheduling on or off + bool force_rotation_angle; // Setting to force wing_angle_deg_sp +}; + +extern struct RotwingState rotwing_state; +extern struct RotWingStateSettings rotwing_state_settings; +extern struct RotWingStateSkewing rotwing_state_skewing; + +extern float rotwing_state_max_hover_speed; + +extern bool hover_motors_active; +extern bool bool_disable_hover_motors; + +extern void init_rotwing_state(void); +extern void periodic_rotwing_state(void); +extern void request_rotwing_state(uint8_t state); +extern void rotwing_request_configuration(uint8_t configuration); +extern void rotwing_state_skew_actuator_periodic(void); +extern void rotwing_state_force_skew_off(void); + +#endif // ROTWING_STATE_H diff --git a/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.c b/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.c index d525110a6b..b79e3d09dc 100644 --- a/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.c +++ b/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.c @@ -71,7 +71,7 @@ void wing_rotation_adc_to_deg(void) // SEND ABI Message to ctr_eff_sched and other modules that want Actuator position feedback struct act_feedback_t feedback = {0}; - feedback.idx = SERVO_ROTATION_MECH_IDX; + feedback.idx = COMMAND_ROT_MECH; feedback.position = 0.5 * M_PI - RadOfDeg(wing_angle_deg); feedback.set.position = true; diff --git a/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.h b/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.h index 736474d748..32dac4452b 100644 --- a/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.h +++ b/sw/airborne/modules/rot_wing_drone/wing_rotation_adc_sensor.h @@ -26,6 +26,7 @@ #ifndef WING_ROTATION_ADC_SENSOR_H #define WING_ROTATION_ADC_SENSOR_H +extern float adc_wing_rotation_extern; extern void wing_rotation_adc_init(void); extern void wing_rotation_adc_to_deg(void);