Skip to content

Commit

Permalink
[ctrl] Morphing quad-plane control eff (#3124)
Browse files Browse the repository at this point in the history
  • Loading branch information
@dewagter
dewagter committed Oct 6, 2023
1 parent 6983ecd commit 5af84a4
Show file tree
Hide file tree
Showing 4 changed files with 344 additions and 1 deletion.
47 changes: 47 additions & 0 deletions conf/modules/ctrl_eff_sched_rot_wing.xml
View file
Original file line number Diff line number Diff line change
@@ -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>
231 changes: 231 additions & 0 deletions sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.c
View file
Original file line number Diff line number Diff line change
@@ -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
}

65 changes: 65 additions & 0 deletions sw/airborne/modules/ctrl/ctrl_eff_sched_rot_wing.h
View file
Original file line number Diff line number Diff line change
@@ -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
2 changes: 1 addition & 1 deletion sw/ext/pprzlink
Submodule pprzlink updated 1 files
+12 −5 message_definitions/v1.0/messages.xml

0 comments on commit 5af84a4

Please sign in to comment.