mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-01 04:46:51 +08:00
[ctrl] Morphing quad-plane control eff (#3124)
This commit is contained in:
Christophe De Wagter
committed by
GitHub
GitHub
parent
6983ecd5bb
commit
5af84a4851
@@ -0,0 +1,47 @@
|
|||||||
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
<module name="ctrl_eff_sched_rot_wing" dir="ctrl">
|
||||||
|
<doc>
|
||||||
|
<description>The control effectiveness scheduler for the rotating wing drone type</description>
|
||||||
|
<section name="ROT_WING" prefix="ROT_WING_EFF_SCHED_">
|
||||||
|
<define name="IXX_BODY" value="0" description=""/>
|
||||||
|
<define name="IYY_BODY" value="0" description=""/>
|
||||||
|
<define name="IZZ" value="0" description=""/>
|
||||||
|
<define name="IXX_WING" value="0" description=""/>
|
||||||
|
<define name="IYY_WING" value="0" description=""/>
|
||||||
|
<define name="M" value="0" description=""/>
|
||||||
|
<define name="ROLL_ARM" value="0" description=""/>
|
||||||
|
<define name="PITCH_ARM" value="0" description=""/>
|
||||||
|
<define name="HOVER_DF_DPPRZ" value="0" description=""/>
|
||||||
|
<define name="HOVER_ROLL_PITCH_COEF" value="{0,0}" description=""/>
|
||||||
|
</section>
|
||||||
|
</doc>
|
||||||
|
<settings>
|
||||||
|
<dl_settings>
|
||||||
|
<dl_settings NAME="Eff_sched">
|
||||||
|
<dl_setting var="rotation_angle_setpoint_deg" min="0" step="1" max="90" shortname="rotation" module="modules/ctrl/ctrl_eff_sched_rot_wing"/>
|
||||||
|
</dl_settings>
|
||||||
|
</dl_settings>
|
||||||
|
</settings>
|
||||||
|
<header>
|
||||||
|
<file name="ctrl_eff_sched_rot_wing.h"/>
|
||||||
|
</header>
|
||||||
|
<init fun="ctrl_eff_sched_rot_wing_init()"/>
|
||||||
|
<periodic fun="ctrl_eff_sched_rot_wing_periodic()" freq="10.0"/>
|
||||||
|
<makefile>
|
||||||
|
<file name="ctrl_eff_sched_rot_wing.c"/>
|
||||||
|
<test>
|
||||||
|
<define name="INDI_NUM_ACT" value="4"/>
|
||||||
|
<define name="INDI_OUTPUTS" value="3"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_IXX_BODY" value="1"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_IYY_BODY" value="1"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_IZZ" value="1"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_IXX_WING" value="1"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_IYY_WING" value="1"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_M" value="1"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_ROLL_ARM" value="1"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_PITCH_ARM" value="1"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_HOVER_DF_DPPRZ" value="1"/>
|
||||||
|
<define name="ROT_WING_EFF_SCHED_HOVER_ROLL_PITCH_COEF" value="{1,1}"/>
|
||||||
|
</test>
|
||||||
|
</makefile>
|
||||||
|
</module>
|
||||||
@@ -0,0 +1,231 @@
|
|||||||
|
/*
|
||||||
|
* 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/ctrl/ctrl_eff_sched_rot_wing.c"
|
||||||
|
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
|
||||||
|
* 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<num_act; i++){
|
||||||
|
if (pos_msg[i].set.position && (pos_msg[i].idx = 7))
|
||||||
|
{
|
||||||
|
// Get wing rotation angle from sensor
|
||||||
|
eff_sched_var.wing_rotation_rad = 0.5 * M_PI - pos_msg[i].position;
|
||||||
|
|
||||||
|
// Bound wing rotation angle
|
||||||
|
Bound(eff_sched_var.wing_rotation_rad, 0, 0.5*M_PI);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
inline void ctrl_eff_sched_rot_wing_update_wing_angle_sp(void);
|
||||||
|
inline void ctrl_eff_sched_rot_wing_update_wing_angle(void);
|
||||||
|
inline void ctrl_eff_sched_rot_wing_update_MMOI(void);
|
||||||
|
inline void ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness(void);
|
||||||
|
|
||||||
|
void ctrl_eff_sched_rot_wing_init(void)
|
||||||
|
{
|
||||||
|
// Initialize variables to quad values
|
||||||
|
eff_sched_var.Ixx = eff_sched_p.Ixx_body + eff_sched_p.Ixx_wing;
|
||||||
|
eff_sched_var.Iyy = eff_sched_p.Iyy_body + eff_sched_p.Iyy_wing;
|
||||||
|
eff_sched_var.wing_rotation_rad = 0;
|
||||||
|
eff_sched_var.cosr = 1;
|
||||||
|
eff_sched_var.sinr = 0;
|
||||||
|
eff_sched_var.cosr2 = 1;
|
||||||
|
eff_sched_var.sinr2 = 0;
|
||||||
|
eff_sched_var.cosr3 = 1;
|
||||||
|
|
||||||
|
// Set moment derivative variables
|
||||||
|
eff_sched_var.pitch_motor_dMdpprz = eff_sched_p.hover_dFdpprz * eff_sched_p.pitch_arm;
|
||||||
|
eff_sched_var.roll_motor_dMdpprz = eff_sched_p.hover_dFdpprz * eff_sched_p.roll_arm;
|
||||||
|
|
||||||
|
AbiBindMsgACT_FEEDBACK(CTRL_EFF_SCHED_ROT_WING_ID, &wing_position_ev, wing_position_cb);
|
||||||
|
|
||||||
|
|
||||||
|
#if PERIODIC_TELEMETRY
|
||||||
|
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ROTATING_WING_STATE, send_rotating_wing_state);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
void ctrl_eff_sched_rot_wing_periodic(void)
|
||||||
|
{
|
||||||
|
// your periodic code here.
|
||||||
|
// freq = 10.0 Hz
|
||||||
|
ctrl_eff_sched_rot_wing_update_wing_angle_sp();
|
||||||
|
ctrl_eff_sched_rot_wing_update_wing_angle();
|
||||||
|
ctrl_eff_sched_rot_wing_update_MMOI();
|
||||||
|
|
||||||
|
// Update the effectiveness values
|
||||||
|
ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void ctrl_eff_sched_rot_wing_update_wing_angle_sp(void)
|
||||||
|
{
|
||||||
|
rotation_cmd = MAX_PPRZ - (int16_t)(rotation_angle_setpoint_deg * pprz_angle_step);
|
||||||
|
// Calulcate rotation_cmd
|
||||||
|
Bound(rotation_cmd, -9600, 9600);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ctrl_eff_sched_rot_wing_update_wing_angle(void)
|
||||||
|
{
|
||||||
|
// Calculate sin and cosines of rotation
|
||||||
|
eff_sched_var.cosr = cosf(eff_sched_var.wing_rotation_rad);
|
||||||
|
eff_sched_var.sinr = sinf(eff_sched_var.wing_rotation_rad);
|
||||||
|
|
||||||
|
eff_sched_var.cosr2 = eff_sched_var.cosr * eff_sched_var.cosr;
|
||||||
|
eff_sched_var.sinr2 = eff_sched_var.sinr * eff_sched_var.sinr;
|
||||||
|
|
||||||
|
eff_sched_var.cosr3 = eff_sched_var.cosr2 * eff_sched_var.cosr;
|
||||||
|
}
|
||||||
|
|
||||||
|
void ctrl_eff_sched_rot_wing_update_MMOI(void)
|
||||||
|
{
|
||||||
|
eff_sched_var.Ixx = eff_sched_p.Ixx_body + eff_sched_var.cosr2 * eff_sched_p.Ixx_wing + eff_sched_var.sinr2 * eff_sched_p.Iyy_wing;
|
||||||
|
eff_sched_var.Iyy = eff_sched_p.Iyy_body + eff_sched_var.sinr2 * eff_sched_p.Ixx_wing + eff_sched_var.cosr2 * eff_sched_p.Iyy_wing;
|
||||||
|
|
||||||
|
// Bound inertia
|
||||||
|
Bound(eff_sched_var.Ixx, 0.01, 100.);
|
||||||
|
Bound(eff_sched_var.Iyy, 0.01, 100.);
|
||||||
|
}
|
||||||
|
|
||||||
|
void ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness(void)
|
||||||
|
{
|
||||||
|
// Pitch motor effectiveness
|
||||||
|
|
||||||
|
float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy;
|
||||||
|
|
||||||
|
// Roll motor effectiveness
|
||||||
|
|
||||||
|
float roll_motor_p_eff = eff_sched_var.roll_motor_dMdpprz * eff_sched_var.cosr / eff_sched_var.Ixx;
|
||||||
|
// float roll_motor_q_eff = eff_sched_var.roll_motor_dMdpprz * eff_sched_var.sinr *
|
||||||
|
// (eff_sched_p.hover_roll_pitch_coef[0] + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.cosr2) / eff_sched_var.Iyy;
|
||||||
|
|
||||||
|
float roll_motor_q_eff = (eff_sched_var.roll_motor_dMdpprz * eff_sched_var.sinr / eff_sched_var.Iyy) * (eff_sched_p.hover_roll_pitch_coef[0] + eff_sched_p.hover_roll_pitch_coef[1] * eff_sched_var.cosr3);
|
||||||
|
|
||||||
|
// Update front pitch motor q effectiveness
|
||||||
|
g1g2[1][0] = pitch_motor_q_eff; // pitch effectiveness front motor
|
||||||
|
|
||||||
|
// Update back motor q effectiveness
|
||||||
|
g1g2[1][2] = -pitch_motor_q_eff; // pitch effectiveness back motor
|
||||||
|
|
||||||
|
// Update right motor p and q effectiveness
|
||||||
|
g1g2[0][1] = -roll_motor_p_eff; // roll effectiveness right motor
|
||||||
|
g1g2[1][1] = roll_motor_q_eff; // pitch effectiveness right motor
|
||||||
|
|
||||||
|
// Update left motor p and q effectiveness
|
||||||
|
g1g2[0][3] = roll_motor_p_eff; // roll effectiveness left motor
|
||||||
|
g1g2[1][3] = -roll_motor_q_eff; // pitch effectiveness left motor
|
||||||
|
}
|
||||||
|
|
||||||
@@ -0,0 +1,65 @@
|
|||||||
|
/*
|
||||||
|
* 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/ctrl/ctrl_eff_sched_rot_wing.h"
|
||||||
|
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
|
||||||
|
* 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
|
||||||
+1
-1
Submodule sw/ext/pprzlink updated: 04658b50fa...cbb8ad25b1
Reference in New Issue
Block a user