118 changes: 118 additions & 0 deletions sw/airborne/modules/ctrl/eff_scheduling_falcon.c
View file
@@ -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);
}

33 changes: 33 additions & 0 deletions sw/airborne/modules/ctrl/eff_scheduling_falcon.h
View file
@@ -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
86 changes: 86 additions & 0 deletions sw/airborne/modules/ctrl/eff_scheduling_generic.c
View file
@@ -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;
}
}
}

22 changes: 8 additions & 14 deletions ...ules/ctrl/ctrl_effectiveness_scheduling.h → ...rne/modules/ctrl/eff_scheduling_generic.h
View file
Expand Up @@ -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 */

102 changes: 51 additions & 51 deletions ...ne/modules/ctrl/ctrl_eff_sched_rot_wing.c → ...ne/modules/ctrl/eff_scheduling_rot_wing.c
View file
Expand Up @@ -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"
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -183,18 +183,18 @@ 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;
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; // ABI input
eff_sched_var.wing_rotation_deg = 0;
eff_sched_var.cosr = 1;
eff_sched_var.sinr = 0;
eff_sched_var.cosr2 = 1;
eff_sched_var.cosr = 1;
eff_sched_var.sinr = 0;
eff_sched_var.cosr2 = 1;
eff_sched_var.sinr2 = 0;
eff_sched_var.sinr3 = 0;
eff_sched_var.sinr3 = 0;

// Set moment derivative variables
eff_sched_var.pitch_motor_dMdpprz = ROT_WING_EFF_SCHED_DM_DPPRZ_HOVER_PITCH;
Expand All @@ -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.;
Expand All @@ -245,34 +245,34 @@ 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;
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_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.);
eff_sched_var.airspeed2 = eff_sched_var.airspeed * eff_sched_var.airspeed;
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
// Pitch motor effectiveness

float pitch_motor_q_eff = eff_sched_var.pitch_motor_dMdpprz / eff_sched_var.Iyy;

Expand Down Expand Up @@ -302,12 +302,12 @@ 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;

float dMyde = (eff_sched_p.k_elevator[0] * de * eff_sched_var.airspeed2 +
eff_sched_p.k_elevator[1] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled * eff_sched_var.airspeed +
eff_sched_p.k_elevator[1] * eff_sched_var.cmd_pusher_scaled * eff_sched_var.cmd_pusher_scaled * eff_sched_var.airspeed +
eff_sched_p.k_elevator[2] * eff_sched_var.airspeed2) / 10000.;

float dMydpprz = dMyde * eff_sched_p.k_elevator_deflection[1];
Expand All @@ -320,10 +320,10 @@ 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 +
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 +
eff_sched_p.k_rudder[2] * eff_sched_var.airspeed2) / 10000.;

// Convert moment to effectiveness
Expand All @@ -338,23 +338,23 @@ 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;
Bound(eff_x_aileron, 0, 0.005)
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;
Bound(eff_x_flap_aileron, 0, 0.005)
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;

Expand All @@ -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;
Expand All @@ -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)
Expand Down Expand Up @@ -423,4 +423,4 @@ void stabilization_indi_set_wls_settings(float use_increment)

du_min_stab_indi[7] = min_pprz_cmd_flap_ail - use_increment*actuators_pprz[7];
#endif //ROTWING_V3B
}
}
11 changes: 6 additions & 5 deletions ...ne/modules/ctrl/ctrl_eff_sched_rot_wing.h → ...ne/modules/ctrl/eff_scheduling_rot_wing.h
View file
Expand Up @@ -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
*/
Expand All @@ -29,8 +29,8 @@
#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 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²]
Expand Down Expand Up @@ -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