diff --git a/conf/modules/ctrl_eff_sched_rot_wing.xml b/conf/modules/ctrl_eff_sched_rot_wing.xml new file mode 100644 index 0000000000..24e1294953 --- /dev/null +++ b/conf/modules/ctrl_eff_sched_rot_wing.xml @@ -0,0 +1,47 @@ + + + + The control effectiveness scheduler for the rotating wing drone type +
+ + + + + + + + + + +
+ + + + + + + + +
+ +
+ + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c b/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c new file mode 100644 index 0000000000..013e1d5a60 --- /dev/null +++ b/sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c @@ -0,0 +1,231 @@ +/* + * Copyright (C) 2023 Dennis van Wijngaarden + * + * 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/ctrl_eff_sched_rot_wing.c" + * @author Dennis van Wijngaarden + * The control effectiveness scheduler for the rotating wing drone type + */ + +#include "modules/ctrl/ctrl_eff_sched_rot_wing.h" + +#include "firmwares/rotorcraft/stabilization/stabilization_indi.h" +#include "modules/core/abi.h" + + +#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 + +#ifndef ROT_WING_EFF_SCHED_ROLL_ARM +#error "NO ROT_WING_EFF_SCHED_ROLL_ARM defined" +#endif + +#ifndef ROT_WING_EFF_SCHED_PITCH_ARM +#error "NO ROT_WING_EFF_SCHED_PITCH_ARM defined" +#endif + +#ifndef ROT_WING_EFF_SCHED_HOVER_DF_DPPRZ +#error "NO ROT_WING_EFF_SCHED_HOVER_DF_DPPRZ defined" +#endif + +#ifndef ROT_WING_EFF_SCHED_HOVER_ROLL_PITCH_COEF +#error "NO ROT_WING_EFF_SCHED_HOVER_ROLL_PITCH_COEF defined" +#endif + +struct rot_wing_eff_sched_param_t eff_sched_p = { + .Ixx_body = ROT_WING_EFF_SCHED_IXX_BODY, + .Iyy_body = ROT_WING_EFF_SCHED_IYY_BODY, + .Izz = ROT_WING_EFF_SCHED_IZZ, + .Ixx_wing = ROT_WING_EFF_SCHED_IXX_WING, + .Iyy_wing = ROT_WING_EFF_SCHED_IYY_WING, + .m = ROT_WING_EFF_SCHED_M, + .roll_arm = ROT_WING_EFF_SCHED_ROLL_ARM, + .pitch_arm = ROT_WING_EFF_SCHED_PITCH_ARM, + .hover_dFdpprz = ROT_WING_EFF_SCHED_HOVER_DF_DPPRZ, + .hover_roll_pitch_coef = ROT_WING_EFF_SCHED_HOVER_ROLL_PITCH_COEF +}; + +struct rot_wing_eff_sched_var_t eff_sched_var; + +float rotation_angle_setpoint_deg = 0; // Quad mode +int16_t rotation_cmd = 9600; // Quad mode +float pprz_angle_step = 9600. / 45.; // CMD per degree + +// Telemetry +#if PERIODIC_TELEMETRY +#include "modules/datalink/telemetry.h" +static void send_rotating_wing_state(struct transport_tx *trans, struct link_device *dev) +{ + uint8_t state = 0; // Quadrotor + float angle = eff_sched_var.wing_rotation_rad / M_PI * 180.f; + pprz_msg_send_ROTATING_WING_STATE(trans, dev, AC_ID, + &state, + &state, + &angle, + &rotation_angle_setpoint_deg, + 0, + 0); +} +#endif + + +/** ABI binding wing position data. + */ +#ifndef CTRL_EFF_SCHED_ROT_WING_ID +#define CTRL_EFF_SCHED_ROT_WING_ID ABI_BROADCAST +#endif +PRINT_CONFIG_VAR(CTRL_EFF_SCHED_ROT_WING_ID) +static abi_event wing_position_ev; + +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 + * + * 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/ctrl_eff_sched_rot_wing.h" + * @author Dennis van Wijngaarden + * 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" + +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 roll_arm; // distance from rotation point to roll motor [m] + float pitch_arm; // distance from rotation point to pitch motor [m] + float hover_dFdpprz; // derivative of delta force with respect to a delta paparazzi command [N/pprz] + float hover_roll_pitch_coef[2]; // Model coefficients to correct pitch effective for roll motors +}; + +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 + 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; // cos³ 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] +}; + +extern float rotation_angle_setpoint_deg; +extern int16_t rotation_cmd; + +extern void ctrl_eff_sched_rot_wing_init(void); +extern void ctrl_eff_sched_rot_wing_periodic(void); + +#endif // CTRL_EFF_SCHED_ROT_WING_H diff --git a/sw/ext/pprzlink b/sw/ext/pprzlink index 04658b50fa..cbb8ad25b1 160000 --- a/sw/ext/pprzlink +++ b/sw/ext/pprzlink @@ -1 +1 @@ -Subproject commit 04658b50fa9fbd9a00a74647d3657f679eb737b7 +Subproject commit cbb8ad25b1a2a185dce3e231b2c1338be14bd86f