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

[indi] support rate mode (#2004)

Browse Source 
Also prevent unused parameter warning for the thrust callback
This commit is contained in:
Ewoud Smeur
2017-02-12 14:52:47 +01:00
committed by Felix Ruess
parent 9792de5704
commit 98ba658b64
1 changed files with 30 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+30 -7
View File
@@ -87,6 +87,11 @@ bool act_is_servo[INDI_NUM_ACT] = {0};
float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
/** Maximum rate you can request in RC rate mode (rad/s)*/
#ifndef STABILIZATION_INDI_MAX_RATE
#define STABILIZATION_INDI_MAX_RATE 6.0
#endif
// variables needed for control
float actuator_state_filt_vect[INDI_NUM_ACT];
struct FloatRates angular_accel_ref = {0., 0., 0.};
@@ -303,13 +308,31 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
*/
static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_control, bool in_flight)
{
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;
} else {
//calculate the virtual control (reference acceleration) based on a PD controller
struct FloatRates rate_ref;
rate_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
/reference_acceleration.rate_p;
rate_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
/reference_acceleration.rate_q;
rate_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
/reference_acceleration.rate_r;
// Possibly we can use some bounding here
/*BoundAbs(rate_ref.r, 5.0);*/
}
struct FloatRates *body_rates = stateGetBodyRates_f();
//calculate the virtual control (reference acceleration) based on a PD controller
angular_accel_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
- reference_acceleration.rate_p * stateGetBodyRates_f()->p;
angular_accel_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
- reference_acceleration.rate_q * stateGetBodyRates_f()->q;
angular_accel_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
- reference_acceleration.rate_r * stateGetBodyRates_f()->r;
angular_accel_ref.p = (rate_ref.p - body_rates->p) * reference_acceleration.rate_p;
angular_accel_ref.q = (rate_ref.q - body_rates->q) * reference_acceleration.rate_q;
angular_accel_ref.r = (rate_ref.r - body_rates->r) * reference_acceleration.rate_r;
g2_times_du = 0.0;
int8_t i;
@@ -652,7 +675,7 @@ static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *r
/**
* ABI callback that obtains the thrust increment from guidance INDI
*/
static void thrust_cb(uint8_t sender_id, float thrust_increment)
static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
{
indi_thrust_increment = thrust_increment;
indi_thrust_increment_set = true;