mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-02 13:27:32 +08:00
[scheduling] harmonize efficiency scheduling modules (#3182)
- harmonize name - make a 'generic' module with basic linear scheduling - old ctrl_effectiveness_scheduling was very specific to cyfoam, so renamed - add falcon scheduler, related airframe will come later
This commit is contained in:
Gautier Hattenberger
committed by
GitHub
GitHub
parent
44a3abb977
commit
68ddea1e3f
@@ -15,7 +15,7 @@
|
||||
<define name="RADIO_KILL_SWITCH" value="RADIO_GAIN1"/>
|
||||
</module>
|
||||
|
||||
<module name="ctrl_effectiveness_scheduling">
|
||||
<module name="eff_scheduling_cyfoam">
|
||||
<define name="SQUARED_ROLL_EFF" value="0.0018"/>
|
||||
<define name="PITCH_EFF_AT_60" value="4.0"/>
|
||||
<define name="YAW_EFF_AT_60" value="8.0"/>
|
||||
|
||||
@@ -188,7 +188,7 @@
|
||||
<define name="RADIO_KILL_SWITCH" value="5"/>
|
||||
</module>
|
||||
|
||||
<module name="ctrl_effectiveness_scheduling">
|
||||
<module name="eff_scheduling_cyfoam">
|
||||
<define name="SQUARED_ROLL_EFF" value="0.0018"/>
|
||||
<define name="PITCH_EFF_AT_60" value="4.0"/>
|
||||
<define name="YAW_EFF_AT_60" value="8.0"/>
|
||||
|
||||
@@ -108,7 +108,7 @@
|
||||
|
||||
<module name="stabilization" type="rate_indi"/>
|
||||
|
||||
<module name="ctrl_eff_sched_rot_wing"/>
|
||||
<module name="eff_scheduling_rot_wing"/>
|
||||
|
||||
<module name="guidance" type="indi_hybrid_quadplane"/>
|
||||
<module name="nav" type="hybrid">
|
||||
|
||||
@@ -108,7 +108,7 @@
|
||||
<define name="WLS_N_V" value="5"/>
|
||||
</module>
|
||||
|
||||
<module name="ctrl_eff_sched_rot_wing"/>
|
||||
<module name="eff_scheduling_rot_wing"/>
|
||||
|
||||
<module name="guidance" type="indi_hybrid_quadplane"/>
|
||||
<module name="nav" type="hybrid">
|
||||
|
||||
@@ -1,22 +0,0 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="ctrl_effectiveness_scheduling" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Interpolation of control effectivenss matrix.
|
||||
This is necessary if the vehicle has different operating points,
|
||||
with significantly different control effectivenss.
|
||||
|
||||
If instead using online adaptation is an option, be sure to
|
||||
not use this module at the same time!
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="ctrl_effectiveness_scheduling.h"/>
|
||||
</header>
|
||||
<init fun="ctrl_eff_scheduling_init()"/>
|
||||
<periodic fun="ctrl_eff_scheduling_periodic()" freq="20"/>
|
||||
<makefile>
|
||||
<file name="ctrl_effectiveness_scheduling.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,24 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="eff_scheduling_cyfoam" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Interpolation of control effectiveness matrix for Cyfoam tailsitter.
|
||||
|
||||
The configuration of Cyfoam is a tailsitter with 2 propellers
|
||||
blowing a wing with 2 flaps. A minimum amont of thrust is thus
|
||||
required to have enough control on moments, especially pitch moment.
|
||||
|
||||
Two scheduling functions are available, controlled by the flag
|
||||
INDI_FUNCTIONS_RC_CHANNEL, which indicates the RC channel to be used.
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="eff_scheduling_cyfoam.h"/>
|
||||
</header>
|
||||
<init fun="eff_scheduling_cyfoam_init()"/>
|
||||
<periodic fun="eff_scheduling_cyfoam_periodic()" freq="20"/>
|
||||
<makefile>
|
||||
<file name="eff_scheduling_cyfoam.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,24 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
<module name="eff_scheduling_falcon" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Interpolation of control effectivenss matrix of the Falcon hybride plane
|
||||
|
||||
The configuation of Falcon is a tailsitter with 4 propellers mounted as a
|
||||
regular quadcopter and a wing in the middle with 2 ailevons. The ailevons
|
||||
are only used for roll and pitch with sufficient forward speed. This module
|
||||
is in charge of disabling them at low speed / high angle of attack when
|
||||
they have no more efficiency.
|
||||
</description>
|
||||
</doc>
|
||||
|
||||
<header>
|
||||
<file name="eff_scheduling_falcon.h"/>
|
||||
</header>
|
||||
<init fun="eff_scheduling_falcon_init()"/>
|
||||
<periodic fun="eff_scheduling_falcon_periodic()" freq="20.0"/>
|
||||
<periodic fun="eff_scheduling_falcon_report()" freq="0.5" autorun="FALSE"/> <!-- TRUE FALSE-->
|
||||
<makefile target="ap|nps" firmware="rotorcraft" >
|
||||
<file name="eff_scheduling_falcon.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -0,0 +1,24 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
|
||||
<module name="eff_scheduling_generic" dir="ctrl">
|
||||
<doc>
|
||||
<description>
|
||||
Interpolation of control effectivenss matrix.
|
||||
This is necessary if the vehicle has different operating points,
|
||||
with significantly different control effectivenss.
|
||||
Interpolation is linear between two points, usually hover and
|
||||
forward conditions.
|
||||
|
||||
If instead using online adaptation is an option, be sure to
|
||||
not use this module at the same time!
|
||||
</description>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="eff_scheduling_generic.h"/>
|
||||
</header>
|
||||
<init fun="eff_scheduling_generic_init()"/>
|
||||
<periodic fun="eff_scheduling_generic_periodic()" freq="20"/>
|
||||
<makefile>
|
||||
<file name="eff_scheduling_generic.c"/>
|
||||
</makefile>
|
||||
</module>
|
||||
@@ -1,7 +1,8 @@
|
||||
<!DOCTYPE module SYSTEM "module.dtd">
|
||||
<module name="ctrl_eff_sched_rot_wing" dir="ctrl">
|
||||
<module name="eff_scheduling_rot_wing" dir="ctrl">
|
||||
<doc>
|
||||
<description>The control effectiveness scheduler for the rotating wing quadplane drone type
|
||||
<description>T
|
||||
he control effectiveness scheduler for the rotating wing quadplane drone type
|
||||
- it requires a servo called ROTATION_MECH
|
||||
</description>
|
||||
<section name="ROT_WING" prefix="ROT_WING_EFF_SCHED_">
|
||||
@@ -18,12 +19,12 @@
|
||||
</section>
|
||||
</doc>
|
||||
<header>
|
||||
<file name="ctrl_eff_sched_rot_wing.h"/>
|
||||
<file name="eff_scheduling_rot_wing.h"/>
|
||||
</header>
|
||||
<init fun="ctrl_eff_sched_rot_wing_init()"/>
|
||||
<periodic fun="ctrl_eff_sched_rot_wing_periodic()" freq="10.0"/>
|
||||
<init fun="eff_scheduling_rot_wing_init()"/>
|
||||
<periodic fun="eff_scheduling_rot_wing_periodic()" freq="10.0"/>
|
||||
<makefile>
|
||||
<file name="ctrl_eff_sched_rot_wing.c"/>
|
||||
<file name="eff_scheduling_rot_wing.c"/>
|
||||
<test>
|
||||
<define name="ACTUATORS_NB" value="10"/>
|
||||
<define name="SERVO_ROTATION_MECH" value="8"/>
|
||||
+41
-38
@@ -14,16 +14,15 @@
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/** @file modules/ctrl/ctrl_effectiveness_scheduling.c
|
||||
* Module that interpolates gainsets in flight based on the transition percentage
|
||||
*/
|
||||
|
||||
#include "modules/ctrl/ctrl_effectiveness_scheduling.h"
|
||||
#include "modules/ctrl/eff_scheduling_cyfoam.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_h.h"
|
||||
#include "generated/airframe.h"
|
||||
@@ -38,18 +37,23 @@
|
||||
#error "You need to define an RC channel to switch between simple and advanced scheduling"
|
||||
#endif
|
||||
|
||||
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {FWD_G1_ROLL,
|
||||
FWD_G1_PITCH, FWD_G1_YAW, FWD_G1_THRUST
|
||||
};
|
||||
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {
|
||||
FWD_G1_ROLL,
|
||||
FWD_G1_PITCH,
|
||||
FWD_G1_YAW,
|
||||
FWD_G1_THRUST
|
||||
};
|
||||
|
||||
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {STABILIZATION_INDI_G1_ROLL,
|
||||
STABILIZATION_INDI_G1_PITCH, STABILIZATION_INDI_G1_YAW, STABILIZATION_INDI_G1_THRUST
|
||||
};
|
||||
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {
|
||||
STABILIZATION_INDI_G1_ROLL,
|
||||
STABILIZATION_INDI_G1_PITCH,
|
||||
STABILIZATION_INDI_G1_YAW,
|
||||
STABILIZATION_INDI_G1_THRUST
|
||||
};
|
||||
|
||||
static float g2_both[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
|
||||
|
||||
//Get the specified gains in the gainlibrary
|
||||
void ctrl_eff_scheduling_init(void)
|
||||
void eff_scheduling_cyfoam_init(void)
|
||||
{
|
||||
//sum of G1 and G2
|
||||
int8_t i;
|
||||
@@ -67,31 +71,7 @@ void ctrl_eff_scheduling_init(void)
|
||||
}
|
||||
}
|
||||
|
||||
void ctrl_eff_scheduling_periodic(void)
|
||||
{
|
||||
if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0) {
|
||||
ctrl_eff_scheduling_periodic_b();
|
||||
} else {
|
||||
ctrl_eff_scheduling_periodic_a();
|
||||
}
|
||||
|
||||
#ifdef INDI_THRUST_ON_PITCH_EFF
|
||||
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
|
||||
if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] < -7000) && (actuator_state_filt_vect[1] > 7000)) {
|
||||
Bwls[1][2] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
|
||||
Bwls[1][3] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
|
||||
} else if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] > 7000) && (actuator_state_filt_vect[1] < -7000)) {
|
||||
Bwls[1][2] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
|
||||
Bwls[1][3] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
|
||||
} else {
|
||||
Bwls[1][2] = 0.0;
|
||||
Bwls[1][3] = 0.0;
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
void ctrl_eff_scheduling_periodic_a(void)
|
||||
static void eff_scheduling_periodic_a(void)
|
||||
{
|
||||
// Go from transition percentage to ratio
|
||||
float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;
|
||||
@@ -105,7 +85,7 @@ void ctrl_eff_scheduling_periodic_a(void)
|
||||
}
|
||||
}
|
||||
|
||||
void ctrl_eff_scheduling_periodic_b(void)
|
||||
static void eff_scheduling_periodic_b(void)
|
||||
{
|
||||
float airspeed = stateGetAirspeed_f();
|
||||
struct FloatEulers eulers_zxy;
|
||||
@@ -149,3 +129,26 @@ void ctrl_eff_scheduling_periodic_b(void)
|
||||
|
||||
}
|
||||
|
||||
void eff_scheduling_cyfoam_periodic(void)
|
||||
{
|
||||
if (radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0) {
|
||||
eff_scheduling_periodic_b();
|
||||
} else {
|
||||
eff_scheduling_periodic_a();
|
||||
}
|
||||
|
||||
#ifdef INDI_THRUST_ON_PITCH_EFF
|
||||
//State prioritization {W Roll, W pitch, W yaw, TOTAL THRUST}
|
||||
if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] < -7000) && (actuator_state_filt_vect[1] > 7000)) {
|
||||
Bwls[1][2] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
|
||||
Bwls[1][3] = INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
|
||||
} else if(radio_control.values[INDI_FUNCTIONS_RC_CHANNEL] > 0 && (actuator_state_filt_vect[0] > 7000) && (actuator_state_filt_vect[1] < -7000)) {
|
||||
Bwls[1][2] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
|
||||
Bwls[1][3] = -INDI_THRUST_ON_PITCH_EFF/INDI_G_SCALING;
|
||||
} else {
|
||||
Bwls[1][2] = 0.0;
|
||||
Bwls[1][3] = 0.0;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
@@ -0,0 +1,39 @@
|
||||
/*
|
||||
* Copyright (C) Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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/eff_scheduling_cyfoam.h
|
||||
*/
|
||||
|
||||
#ifndef EFF_SCHEDULING_CYFOAM_H
|
||||
#define EFF_SCHEDULING_CYFOAM_H
|
||||
|
||||
/**
|
||||
* Initialises periodic loop;
|
||||
*/
|
||||
extern void eff_scheduling_cyfoam_init(void);
|
||||
|
||||
/**
|
||||
* Periodic function that interpolates between gain sets depending on the scheduling variable.
|
||||
*/
|
||||
extern void eff_scheduling_cyfoam_periodic(void);
|
||||
|
||||
#endif /* EFF_SCHEDULING_CYFOAM_H */
|
||||
|
||||
@@ -0,0 +1,118 @@
|
||||
/*
|
||||
* Copyright (C) 2023 Florian Sansou <florian.sansou@enac.fr>
|
||||
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger.fr>
|
||||
*
|
||||
* 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/eff_scheduling_falcon.c"
|
||||
* Interpolation of control effectivenss matrix of the Falcon hybrid plane
|
||||
*/
|
||||
|
||||
#include "modules/ctrl/eff_scheduling_falcon.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_h.h"
|
||||
#include "state.h"
|
||||
|
||||
#include "modules/datalink/downlink.h"
|
||||
|
||||
// Airspeed at which only with motor
|
||||
#ifndef EFF_SCHEDULING_FALCON_LOW_AIRSPEED
|
||||
#define EFF_SCHEDULING_FALCON_LOW_AIRSPEED 8.0f
|
||||
#endif
|
||||
|
||||
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {
|
||||
STABILIZATION_INDI_G1_ROLL,
|
||||
STABILIZATION_INDI_G1_PITCH,
|
||||
STABILIZATION_INDI_G1_YAW,
|
||||
STABILIZATION_INDI_G1_THRUST
|
||||
};
|
||||
|
||||
void eff_scheduling_falcon_init(void)
|
||||
{
|
||||
for (int8_t i = 0; i < INDI_OUTPUTS; i++) {
|
||||
for (int8_t j = 0; j < INDI_NUM_ACT; j++) {
|
||||
g1g2[i][j] = g1g2_hover[i][j] / INDI_G_SCALING;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void eff_scheduling_falcon_periodic(void)
|
||||
{
|
||||
// calculate squared airspeed
|
||||
float airspeed = stateGetAirspeed_f();
|
||||
|
||||
if (airspeed > EFF_SCHEDULING_FALCON_LOW_AIRSPEED) {
|
||||
airspeed -= EFF_SCHEDULING_FALCON_LOW_AIRSPEED; //offset for start eff at zero!
|
||||
struct FloatEulers eulers_zxy;
|
||||
float_eulers_of_quat_zxy(&eulers_zxy, stateGetNedToBodyQuat_f());
|
||||
|
||||
float pitch_ratio = 0.0f;
|
||||
if (eulers_zxy.theta > -M_PI_4) {
|
||||
pitch_ratio = 0.0f;
|
||||
}
|
||||
else {
|
||||
pitch_ratio = fabsf(1.0f - eulers_zxy.theta/(float)(-M_PI_4));
|
||||
}
|
||||
Bound(pitch_ratio, 0.0f, 1.0f);
|
||||
|
||||
|
||||
Bound(airspeed, 0.0f, 30.0f);
|
||||
float airspeed2 = airspeed*airspeed;
|
||||
|
||||
// not effect of elevon on body roll axis
|
||||
g1g2[0][4] = 0;
|
||||
g1g2[0][5] = 0;
|
||||
|
||||
float pitch_eff = pitch_ratio * EFF_PITCH_A * airspeed2;
|
||||
g1g2[1][4] = pitch_eff / 1000; // elevon_right
|
||||
g1g2[1][5] = -pitch_eff / 1000; // elevon_left
|
||||
|
||||
float yaw_eff = pitch_ratio * EFF_YAW_A * airspeed2;
|
||||
g1g2[2][4] = -yaw_eff / 1000; // elevon_right
|
||||
g1g2[2][5] = -yaw_eff / 1000; // elevon_left
|
||||
|
||||
// No thrust generated by elevon, maybe take drag in accout for the future ?
|
||||
g1g2[3][4] = 0;
|
||||
g1g2[3][5] = 0;
|
||||
}
|
||||
else {
|
||||
//Come back to motor control
|
||||
g1g2[0][4] = 0; // elevon_left
|
||||
g1g2[0][5] = 0; // elevon_right
|
||||
|
||||
g1g2[1][4] = 0; // elevon_left
|
||||
g1g2[1][5] = 0; // elevon_right
|
||||
|
||||
g1g2[2][4] = 0; // elevon_left
|
||||
g1g2[2][5] = 0; // elevon_right
|
||||
|
||||
g1g2[3][4] = 0; // elevon_left
|
||||
g1g2[3][5] = 0; // elevon_right
|
||||
}
|
||||
}
|
||||
|
||||
extern void eff_scheduling_falcon_report(void)
|
||||
{
|
||||
float f[6] = {
|
||||
g1g2[1][4], g1g2[1][5],
|
||||
g1g2[2][4], g1g2[2][5],
|
||||
g1g2[1][0], g1g2[2][0]
|
||||
};
|
||||
DOWNLINK_SEND_PAYLOAD_FLOAT(DefaultChannel, DefaultDevice, 6, f);
|
||||
}
|
||||
|
||||
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
* Copyright (C) 2023 Florian Sansou <florian.sansou@enac.fr>
|
||||
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger.fr>
|
||||
*
|
||||
* 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/eff_scheduling_falcon.h"
|
||||
* Interpolation of control effectivenss matrix of the Falcon hybrid plane
|
||||
*/
|
||||
|
||||
#ifndef EFF_SCHEDULING_FALCON_H
|
||||
#define EFF_SCHEDULING_FALCON_H
|
||||
|
||||
extern void eff_scheduling_falcon_init(void);
|
||||
extern void eff_scheduling_falcon_periodic(void);
|
||||
extern void eff_scheduling_falcon_report(void);
|
||||
|
||||
#endif // EFF_SCHEDULING_FALCON_H
|
||||
@@ -0,0 +1,86 @@
|
||||
/*
|
||||
* Copyright (C) 2017 Ewoud Smeur <ewoud_smeur@msn.com>
|
||||
*
|
||||
* 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/eff_scheduling_generic.c
|
||||
* Module that interpolates gainsets in flight based on the transition percentage
|
||||
*/
|
||||
|
||||
#include "modules/ctrl/ctrl_effectiveness_scheduling.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
|
||||
#include "firmwares/rotorcraft/guidance/guidance_h.h"
|
||||
#include "generated/airframe.h"
|
||||
#include "state.h"
|
||||
#include "modules/radio_control/radio_control.h"
|
||||
|
||||
#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
|
||||
#error "You need to use WLS control allocation for this module"
|
||||
#endif
|
||||
|
||||
#ifndef INDI_FUNCTIONS_RC_CHANNEL
|
||||
#error "You need to define an RC channel to switch between simple and advanced scheduling"
|
||||
#endif
|
||||
|
||||
static float g1g2_forward[INDI_OUTPUTS][INDI_NUM_ACT] = {
|
||||
FWD_G1_ROLL,
|
||||
FWD_G1_PITCH,
|
||||
FWD_G1_YAW,
|
||||
FWD_G1_THRUST
|
||||
};
|
||||
|
||||
static float g1g2_hover[INDI_OUTPUTS][INDI_NUM_ACT] = {
|
||||
STABILIZATION_INDI_G1_ROLL,
|
||||
STABILIZATION_INDI_G1_PITCH,
|
||||
STABILIZATION_INDI_G1_YAW,
|
||||
STABILIZATION_INDI_G1_THRUST
|
||||
};
|
||||
|
||||
static float g2_both[INDI_NUM_ACT] = STABILIZATION_INDI_G2; //scaled by INDI_G_SCALING
|
||||
|
||||
void eff_scheduling_generic_init(void)
|
||||
{
|
||||
//sum of G1 and G2
|
||||
int8_t i;
|
||||
int8_t j;
|
||||
for (i = 0; i < INDI_OUTPUTS; i++) {
|
||||
for (j = 0; j < INDI_NUM_ACT; j++) {
|
||||
if (i != 2) {
|
||||
g1g2_hover[i][j] = g1g2_hover[i][j] / INDI_G_SCALING;
|
||||
g1g2_forward[i][j] = g1g2_forward[i][j] / INDI_G_SCALING;
|
||||
} else {
|
||||
g1g2_forward[i][j] = (g1g2_forward[i][j] + g2_both[j]) / INDI_G_SCALING;
|
||||
g1g2_hover[i][j] = (g1g2_hover[i][j] + g2_both[j]) / INDI_G_SCALING;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void eff_scheduling_generic_periodic(void)
|
||||
{
|
||||
// Go from transition percentage to ratio
|
||||
float ratio = FLOAT_OF_BFP(transition_percentage, INT32_PERCENTAGE_FRAC) / 100;
|
||||
int8_t i;
|
||||
int8_t j;
|
||||
for (i = 0; i < INDI_OUTPUTS; i++) {
|
||||
for (j = 0; j < INDI_NUM_ACT; j++) {
|
||||
g1g2[i][j] = g1g2_hover[i][j] * (1.0 - ratio) + g1g2_forward[i][j] * ratio;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
+8
-14
@@ -14,32 +14,26 @@
|
||||
* 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, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file modules/ctrl/ctrl_effectiveness_scheduling.h
|
||||
* @file modules/ctrl/eff_scheduling_generic.h
|
||||
*/
|
||||
|
||||
#ifndef CTRL_EFFECTIVENESS_SCHEDULING_H
|
||||
#define CTRL_EFFECTIVENESS_SCHEDULING_H
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
|
||||
#ifndef EFF_SCHEDULING_GENERIC_H
|
||||
#define EFF_SCHEDULING_GENERIC_H
|
||||
|
||||
/**
|
||||
* Initialises periodic loop;
|
||||
*/
|
||||
extern void ctrl_eff_scheduling_init(void);
|
||||
extern void eff_scheduling_generic_init(void);
|
||||
|
||||
/**
|
||||
* Periodic function that interpolates between gain sets depending on the scheduling variable.
|
||||
*/
|
||||
extern void ctrl_eff_scheduling_periodic(void);
|
||||
extern void ctrl_eff_scheduling_periodic_a(void);
|
||||
extern void ctrl_eff_scheduling_periodic_b(void);
|
||||
extern void eff_scheduling_generic_periodic(void);
|
||||
|
||||
#endif /* CTRL_EFFECTIVENESS_SCHEDULING_H */
|
||||
#endif /* EFF_SCHEDULING_GENERIC_H */
|
||||
|
||||
+40
-40
@@ -18,12 +18,12 @@
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/** @file "modules/ctrl/ctrl_eff_sched_rot_wing.c"
|
||||
/** @file "modules/ctrl/eff_scheduling_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 "modules/ctrl/eff_scheduling_rot_wing.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "state.h"
|
||||
@@ -145,17 +145,17 @@ struct rot_wing_eff_sched_param_t eff_sched_p = {
|
||||
|
||||
struct rot_wing_eff_sched_var_t eff_sched_var;
|
||||
|
||||
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_cmd(void);
|
||||
inline void ctrl_eff_sched_rot_wing_update_airspeed(void);
|
||||
inline void ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness(void);
|
||||
inline void ctrl_eff_sched_rot_wing_update_elevator_effectiveness(void);
|
||||
inline void ctrl_eff_sched_rot_wing_update_rudder_effectiveness(void);
|
||||
inline void ctrl_eff_sched_rot_wing_update_aileron_effectiveness(void);
|
||||
inline void ctrl_eff_sched_rot_wing_update_flaperon_effectiveness(void);
|
||||
inline void ctrl_eff_sched_rot_wing_update_pusher_effectiveness(void);
|
||||
inline void ctrl_eff_sched_rot_wing_schedule_liftd(void);
|
||||
inline void eff_scheduling_rot_wing_update_wing_angle(void);
|
||||
inline void eff_scheduling_rot_wing_update_MMOI(void);
|
||||
inline void eff_scheduling_rot_wing_update_cmd(void);
|
||||
inline void eff_scheduling_rot_wing_update_airspeed(void);
|
||||
inline void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void);
|
||||
inline void eff_scheduling_rot_wing_update_elevator_effectiveness(void);
|
||||
inline void eff_scheduling_rot_wing_update_rudder_effectiveness(void);
|
||||
inline void eff_scheduling_rot_wing_update_aileron_effectiveness(void);
|
||||
inline void eff_scheduling_rot_wing_update_flaperon_effectiveness(void);
|
||||
inline void eff_scheduling_rot_wing_update_pusher_effectiveness(void);
|
||||
inline void eff_scheduling_rot_wing_schedule_liftd(void);
|
||||
|
||||
inline float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED);
|
||||
inline void stabilization_indi_set_wls_settings(float use_increment);
|
||||
@@ -183,7 +183,7 @@ static void wing_position_cb(uint8_t sender_id UNUSED, struct act_feedback_t *po
|
||||
}
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_init(void)
|
||||
void eff_scheduling_rot_wing_init(void)
|
||||
{
|
||||
// Initialize variables to quad values
|
||||
eff_sched_var.Ixx = eff_sched_p.Ixx_body + eff_sched_p.Ixx_wing;
|
||||
@@ -211,26 +211,26 @@ void ctrl_eff_sched_rot_wing_init(void)
|
||||
AbiBindMsgACT_FEEDBACK(WING_ROTATION_CAN_ROT_WING_ID, &wing_position_ev, wing_position_cb);
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_periodic(void)
|
||||
void eff_scheduling_rot_wing_periodic(void)
|
||||
{
|
||||
// your periodic code here.
|
||||
// freq = 10.0 Hz
|
||||
ctrl_eff_sched_rot_wing_update_wing_angle();
|
||||
ctrl_eff_sched_rot_wing_update_MMOI();
|
||||
ctrl_eff_sched_rot_wing_update_cmd();
|
||||
ctrl_eff_sched_rot_wing_update_airspeed();
|
||||
eff_scheduling_rot_wing_update_wing_angle();
|
||||
eff_scheduling_rot_wing_update_MMOI();
|
||||
eff_scheduling_rot_wing_update_cmd();
|
||||
eff_scheduling_rot_wing_update_airspeed();
|
||||
|
||||
// Update the effectiveness values
|
||||
ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness();
|
||||
ctrl_eff_sched_rot_wing_update_elevator_effectiveness();
|
||||
ctrl_eff_sched_rot_wing_update_rudder_effectiveness();
|
||||
ctrl_eff_sched_rot_wing_update_aileron_effectiveness();
|
||||
ctrl_eff_sched_rot_wing_update_flaperon_effectiveness();
|
||||
ctrl_eff_sched_rot_wing_update_pusher_effectiveness();
|
||||
ctrl_eff_sched_rot_wing_schedule_liftd();
|
||||
eff_scheduling_rot_wing_update_hover_motor_effectiveness();
|
||||
eff_scheduling_rot_wing_update_elevator_effectiveness();
|
||||
eff_scheduling_rot_wing_update_rudder_effectiveness();
|
||||
eff_scheduling_rot_wing_update_aileron_effectiveness();
|
||||
eff_scheduling_rot_wing_update_flaperon_effectiveness();
|
||||
eff_scheduling_rot_wing_update_pusher_effectiveness();
|
||||
eff_scheduling_rot_wing_schedule_liftd();
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_wing_angle(void)
|
||||
void eff_scheduling_rot_wing_update_wing_angle(void)
|
||||
{
|
||||
// Calculate sin and cosines of rotation
|
||||
eff_sched_var.wing_rotation_deg = eff_sched_var.wing_rotation_rad / M_PI * 180.;
|
||||
@@ -245,7 +245,7 @@ void ctrl_eff_sched_rot_wing_update_wing_angle(void)
|
||||
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_MMOI(void)
|
||||
void eff_scheduling_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;
|
||||
@@ -255,14 +255,14 @@ void ctrl_eff_sched_rot_wing_update_MMOI(void)
|
||||
Bound(eff_sched_var.Iyy, 0.01, 100.);
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_cmd(void)
|
||||
void eff_scheduling_rot_wing_update_cmd(void)
|
||||
{
|
||||
eff_sched_var.cmd_elevator = actuators_pprz[SERVO_SERVO_ELEVATOR];
|
||||
eff_sched_var.cmd_pusher_scaled = actuators_pprz[SERVO_MOTOR_PUSH] * 0.000853229; // Scaled with 8181 / 9600 / 1000
|
||||
eff_sched_var.cmd_T_mean_scaled = (actuators_pprz[SERVO_MOTOR_FRONT] + actuators_pprz[SERVO_MOTOR_RIGHT] + actuators_pprz[SERVO_MOTOR_BACK] + actuators_pprz[SERVO_MOTOR_LEFT]) / 4. * 0.000853229; // Scaled with 8181 / 9600 / 1000
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_airspeed(void)
|
||||
void eff_scheduling_rot_wing_update_airspeed(void)
|
||||
{
|
||||
eff_sched_var.airspeed = stateGetAirspeed_f();
|
||||
Bound(eff_sched_var.airspeed, 0. , 30.);
|
||||
@@ -270,7 +270,7 @@ void ctrl_eff_sched_rot_wing_update_airspeed(void)
|
||||
Bound(eff_sched_var.airspeed2, 0. , 900.);
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness(void)
|
||||
void eff_scheduling_rot_wing_update_hover_motor_effectiveness(void)
|
||||
{
|
||||
// Pitch motor effectiveness
|
||||
|
||||
@@ -302,7 +302,7 @@ void ctrl_eff_sched_rot_wing_update_hover_motor_effectiveness(void)
|
||||
g1g2[1][3] = -roll_motor_q_eff; // pitch effectiveness left motor
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_elevator_effectiveness(void)
|
||||
void eff_scheduling_rot_wing_update_elevator_effectiveness(void)
|
||||
{
|
||||
float de = eff_sched_p.k_elevator_deflection[0] + eff_sched_p.k_elevator_deflection[1] * eff_sched_var.cmd_elevator;
|
||||
|
||||
@@ -320,7 +320,7 @@ void ctrl_eff_sched_rot_wing_update_elevator_effectiveness(void)
|
||||
g1g2[1][5] = eff_y_elev;
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_rudder_effectiveness(void)
|
||||
void eff_scheduling_rot_wing_update_rudder_effectiveness(void)
|
||||
{
|
||||
float dMzdr = (eff_sched_p.k_rudder[0] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_T_mean_scaled +
|
||||
eff_sched_p.k_rudder[1] * eff_sched_var.cmd_T_mean_scaled * eff_sched_var.airspeed2 * eff_sched_var.cosr +
|
||||
@@ -338,7 +338,7 @@ void ctrl_eff_sched_rot_wing_update_rudder_effectiveness(void)
|
||||
g1g2[2][4] = eff_z_rudder;
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_aileron_effectiveness(void)
|
||||
void eff_scheduling_rot_wing_update_aileron_effectiveness(void)
|
||||
{
|
||||
float dMxdpprz = (eff_sched_p.k_aileron * eff_sched_var.airspeed2 * eff_sched_var.sinr3) / 1000000.;
|
||||
float eff_x_aileron = dMxdpprz / eff_sched_var.Ixx;
|
||||
@@ -346,7 +346,7 @@ void ctrl_eff_sched_rot_wing_update_aileron_effectiveness(void)
|
||||
g1g2[0][6] = eff_x_aileron;
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_flaperon_effectiveness(void)
|
||||
void eff_scheduling_rot_wing_update_flaperon_effectiveness(void)
|
||||
{
|
||||
float dMxdpprz = (eff_sched_p.k_flaperon * eff_sched_var.airspeed2 * eff_sched_var.sinr3) / 1000000.;
|
||||
float eff_x_flap_aileron = dMxdpprz / eff_sched_var.Ixx;
|
||||
@@ -354,7 +354,7 @@ void ctrl_eff_sched_rot_wing_update_flaperon_effectiveness(void)
|
||||
g1g2[0][7] = eff_x_flap_aileron;
|
||||
}
|
||||
|
||||
void ctrl_eff_sched_rot_wing_update_pusher_effectiveness(void)
|
||||
void eff_scheduling_rot_wing_update_pusher_effectiveness(void)
|
||||
{
|
||||
float rpmP = eff_sched_p.k_rpm_pprz_pusher[0] + eff_sched_p.k_rpm_pprz_pusher[1] * eff_sched_var.cmd_pusher_scaled + eff_sched_p.k_rpm_pprz_pusher[2] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled;
|
||||
|
||||
@@ -367,9 +367,9 @@ void ctrl_eff_sched_rot_wing_update_pusher_effectiveness(void)
|
||||
g1g2[4][8] = eff_pusher;
|
||||
}
|
||||
|
||||
float ctrl_eff_sched_rot_wing_lift_d = 0.0f;
|
||||
float eff_scheduling_rot_wing_lift_d = 0.0f;
|
||||
|
||||
void ctrl_eff_sched_rot_wing_schedule_liftd(void)
|
||||
void eff_scheduling_rot_wing_schedule_liftd(void)
|
||||
{
|
||||
float lift_d_wing = (eff_sched_p.k_lift_wing[0] + eff_sched_p.k_lift_wing[1] * eff_sched_var.sinr2) * eff_sched_var.airspeed2 / eff_sched_p.m;
|
||||
float lift_d_fuselage = eff_sched_p.k_lift_fuselage * eff_sched_var.airspeed2 / eff_sched_p.m;
|
||||
@@ -380,12 +380,12 @@ void ctrl_eff_sched_rot_wing_schedule_liftd(void)
|
||||
lift_d = 0.0;
|
||||
}
|
||||
Bound(lift_d, -130., 0.);
|
||||
ctrl_eff_sched_rot_wing_lift_d = lift_d;
|
||||
eff_scheduling_rot_wing_lift_d = lift_d;
|
||||
}
|
||||
|
||||
// Override standard LIFT_D function
|
||||
float guidance_indi_get_liftd(float pitch UNUSED, float theta UNUSED) {
|
||||
return ctrl_eff_sched_rot_wing_lift_d;
|
||||
return eff_scheduling_rot_wing_lift_d;
|
||||
}
|
||||
|
||||
void stabilization_indi_set_wls_settings(float use_increment)
|
||||
+4
-3
@@ -18,7 +18,7 @@
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/** @file "modules/ctrl/ctrl_eff_sched_rot_wing.h"
|
||||
/** @file "modules/ctrl/eff_scheduling_rot_wing.h"
|
||||
* @author Dennis van Wijngaarden <D.C.vanWijngaarden@tudelft.nl>
|
||||
* The control effectiveness scheduler for the rotating wing drone type
|
||||
*/
|
||||
@@ -78,7 +78,8 @@ struct rot_wing_eff_sched_var_t {
|
||||
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);
|
||||
extern void eff_scheduling_rot_wing_init(void);
|
||||
extern void eff_scheduling_rot_wing_periodic(void);
|
||||
|
||||
#endif // CTRL_EFF_SCHED_ROT_WING_H
|
||||
|
||||
Reference in New Issue
Block a user