Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 03:43:26 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[INDI Rate Control] - Done (Needs to be checked/verified/validated)

Browse Source
This commit is contained in:
Rohan
2020-08-15 11:30:54 +02:00
committed by Ewoud Smeur
parent 6ad7fa93d0
commit 0203ebec6b
4 changed files with 90 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+6 -3
View File
@@ -35,6 +35,9 @@
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
// Added to access variable "stabilization_rate_sp" for direct rate control
#include "firmwares/rotorcraft/stabilization/stabilization_rate_indi.h"
#include "math/pprz_algebra_float.h"
#include "state.h"
#include "generated/airframe.h"
@@ -350,9 +353,9 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
struct FloatRates rate_ref;
if (rate_control) { //Check if we are running the rate controller
rate_ref.p = (float)radio_control.values[RADIO_ROLL] / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
rate_ref.q = (float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
rate_ref.r = (float)radio_control.values[RADIO_YAW] / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
rate_ref.p = stabilization_rate_sp.p;
rate_ref.q = stabilization_rate_sp.q;
rate_ref.r = stabilization_rate_sp.r;
} else {
//calculate the virtual control (reference acceleration) based on a PD controller
rate_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+6 -3
View File
@@ -35,6 +35,9 @@
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
// Added to access variable "stabilization_rate_sp" for direct rate control
#include "firmwares/rotorcraft/stabilization/stabilization_rate_indi.h"
#include "state.h"
#include "generated/airframe.h"
#include "paparazzi.h"
@@ -327,9 +330,9 @@ static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct I
/* Check if we are running the rate controller and overwrite */
if (rate_control) {
indi.angular_accel_ref.p = indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL] / MAX_PPRZ * indi.max_rate - body_rates->p);
indi.angular_accel_ref.q = indi.reference_acceleration.rate_q * ((float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * indi.max_rate - body_rates->q);
indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * ((float)radio_control.values[RADIO_YAW] / MAX_PPRZ * indi.max_rate - body_rates->r);
indi.angular_accel_ref.p = indi.reference_acceleration.rate_p * (stabilization_rate_sp.p - body_rates->p);
indi.angular_accel_ref.q = indi.reference_acceleration.rate_q * (stabilization_rate_sp.q - body_rates->q);
indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (stabilization_rate_sp.r - body_rates->r);
}
//Increment in angular acceleration requires increment in control input
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c
+76 -4
View File
@@ -23,7 +23,7 @@
* Rate stabilization for rotorcrafts based on INDI by Ewoud Smeur.
*/
#include "firmwares/rotorcraft/stabilization.h"
#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
#include "firmwares/rotorcraft/stabilization/stabilization_rate_indi.h"
#ifdef STABILIZATION_ATTITUDE_INDI_SIMPLE
#include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.h"
@@ -31,21 +31,93 @@
#include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
#endif
struct FloatRates stabilization_rate_sp;
#ifndef STABILIZATION_RATE_DEADBAND_P
#define STABILIZATION_RATE_DEADBAND_P 0
#endif
#ifndef STABILIZATION_RATE_DEADBAND_Q
#define STABILIZATION_RATE_DEADBAND_Q 0
#endif
#ifndef STABILIZATION_RATE_DEADBAND_R
#define STABILIZATION_RATE_DEADBAND_R 200
#endif
#define ROLL_RATE_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_ROLL] > STABILIZATION_RATE_DEADBAND_P || \
radio_control.values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)
#define PITCH_RATE_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_PITCH] > STABILIZATION_RATE_DEADBAND_Q || \
radio_control.values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)
#define YAW_RATE_DEADBAND_EXCEEDED() \
(radio_control.values[RADIO_YAW] > STABILIZATION_RATE_DEADBAND_R || \
radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_rate(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_RATE_LOOP(trans, dev, AC_ID,
&stabilization_rate_sp.p,
&stabilization_rate_sp.q,
&stabilization_rate_sp.r,
&stabilization_cmd[COMMAND_THRUST]);
}
#endif
void stabilization_rate_init(void)
{
stabilization_indi_init();
}
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RATE_LOOP, send_rate);
#endif
}
void stabilization_rate_read_rc(void)
{
//FIXME: make a new indi function
if (ROLL_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.p = (float) radio_control.values[RADIO_ROLL] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
} else {
stabilization_rate_sp.p = 0;
}
if (PITCH_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.q = (float) radio_control.values[RADIO_PITCH] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
} else {
stabilization_rate_sp.q = 0;
}
if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
stabilization_rate_sp.r = (float) radio_control.values[RADIO_YAW] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
} else {
stabilization_rate_sp.r = 0;
}
}
//Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
void stabilization_rate_read_rc_switched_sticks(void)
{
//FIXME: make a new indi function
if (ROLL_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.r = - (float) radio_control.values[RADIO_ROLL] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
} else {
stabilization_rate_sp.r = 0;
}
if (PITCH_RATE_DEADBAND_EXCEEDED()) {
stabilization_rate_sp.q = (float) radio_control.values[RADIO_PITCH] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
} else {
stabilization_rate_sp.q = 0;
}
if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
stabilization_rate_sp.p = (float) radio_control.values[RADIO_YAW] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
} else {
stabilization_rate_sp.p = 0;
}
}
void stabilization_rate_enter(void)
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.h
+2
View File
@@ -34,4 +34,6 @@ extern void stabilization_rate_read_rc_switched_sticks(void);
extern void stabilization_rate_run(bool in_flight);
extern void stabilization_rate_enter(void);
extern struct FloatRates stabilization_rate_sp;
#endif /* STABILIZATION_RATE_INDI */