diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index b0c5d2800b7..2a2dd2c9662 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -35,9 +35,6 @@ #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" @@ -349,7 +346,7 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) * * Function that calculates the INDI commands */ -static void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight) +void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight) { struct FloatRates *body_rates = stateGetBodyRates_f(); @@ -479,7 +476,6 @@ static void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flig */ void stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp) { - /* Propagate the filter on the gyroscopes */ struct FloatRates *body_rates = stateGetBodyRates_f(); float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r}; @@ -501,7 +497,7 @@ void stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp) /* attitude error */ struct Int32Quat att_err; struct Int32Quat *att_quat = stateGetNedToBodyQuat_i(); - int32_quat_inv_comp(&att_err, att_quat, &quat_sp); // [quat_sp is stab_att_sp_quat -- HOW TO DISTINGUISH WHAT YOU GET FROM RC AND SET BY USER FROM ANOTHER MODULE?] + int32_quat_inv_comp(&att_err, att_quat, &quat_sp); /* wrap it in the shortest direction */ int32_quat_wrap_shortest(&att_err); int32_quat_normalize(&att_err); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h index 0dcba73361f..d51610412a1 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h @@ -54,7 +54,8 @@ extern void stabilization_indi_enter(void); extern void stabilization_indi_set_failsafe_setpoint(void); extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy); extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading); -extern stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp) +extern void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight) +extern void stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp) extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn); #endif /* STABILIZATION_INDI */ diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c index 3d627b98c0f..adceff0c275 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c @@ -35,9 +35,6 @@ #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" @@ -78,7 +75,7 @@ struct Int32Eulers stab_att_sp_euler; struct Int32Quat stab_att_sp_quat; static int32_t stabilization_att_indi_cmd[COMMANDS_NB]; -static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], bool in_flight); +static inline void stabilization_indi_calc_cmd(int32_t indi_commands[]); static inline void lms_estimation(void); static void indi_init_filters(void); @@ -291,7 +288,7 @@ static inline void finite_difference(float output[3], float new[3], float old[3] * @param att_err quaternion attitude error * @param rate_control rate control enabled, otherwise attitude control */ -static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], bool in_flight __attribute__((unused))) +void stabilization_indi_calc_cmd(int32_t indi_commands[]) { //Increment in angular acceleration requires increment in control input //G1 is the control effectiveness. In the yaw axis, we need something additional: G2. @@ -349,10 +346,10 @@ static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], bool in_ * @param in_flight not used * @param rate_control rate control enabled, otherwise attitude control */ -void stabilization_indi_attitude_run(bool in_flight __attribute__((unused)), struct Int32Quat quat_sp) // how does quat_sp is set when using RC? +void stabilization_indi_attitude_run(bool in_flight __attribute__((unused)), struct Int32Quat quat_sp) { /* attitude error */ - struct Int32Quat att_err; + struct Int32Quat att_err; struct Int32Quat *att_quat = stateGetNedToBodyQuat_i(); int32_quat_inv_comp(&att_err, att_quat, &quat_sp); /* wrap it in the shortest direction */ @@ -395,7 +392,7 @@ void stabilization_indi_attitude_run(bool in_flight __attribute__((unused)), str indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref_r - rates_for_feedback.r); /* compute the INDI command */ - stabilization_indi_calc_cmd(stabilization_att_indi_cmd, in_flight); + stabilization_indi_calc_cmd(stabilization_att_indi_cmd); /* copy the INDI command */ stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL]; diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h index 67cda4bd81c..41ae27f9eae 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h @@ -88,6 +88,7 @@ extern void stabilization_indi_enter(void); extern void stabilization_indi_set_failsafe_setpoint(void); extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy); extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading); +extern void stabilization_indi_calc_cmd(int32_t indi_commands[]) extern void stabilization_indi_run(bool in_flight, struct Int32Quat quat_sp); extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c index d9eb4d3dd7f..6f1dd84fc47 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2016 Freek van Tienen + * 2020 Rohan Chotalal * * This file is part of paparazzi. * @@ -22,6 +23,7 @@ /** @file stabilization_rate_indi.c * Rate stabilization for rotorcrafts based on INDI by Ewoud Smeur. */ + #include "firmwares/rotorcraft/stabilization.h" #include "firmwares/rotorcraft/stabilization/stabilization_rate_indi.h" @@ -127,33 +129,37 @@ void stabilization_rate_set_setpoint_i(struct Int32Rates *pqr) RATES_FLOAT_OF_BFP(stabilization_rate_sp, *pqr); } -static void stabilization_indi_rate_calc_cmd(int32_t indi_commands[], struct FloatRates rate_ref, bool in_flight) +static void stabilization_indi_rate_calc_cmd(int32_t indi_commands[], bool in_flight) { /* Get the angular acceleration references */ indi.angular_accel_ref.p = indi.reference_acceleration.rate_p * (rate_ref.p - body_rates->p); indi.angular_accel_ref.q = indi.reference_acceleration.rate_q * (rate_ref.q - body_rates->q); indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref.r - body_rates->r); +// WE NEED TO CHOOSE BETWEEN STABILIZATION_INDI_SIMPLE AND STABILIZATION_INDI - NOT SURE IF I DID CORRECTLY HERE: +#ifdef STABILIZATION_ATTITUDE_INDI_SIMPLE stabilization_indi_calc_cmd(indi_commands) + /* copy the INDI command */ + stabilization_cmd[COMMAND_ROLL] = stabilization_att_indi_cmd[COMMAND_ROLL]; + stabilization_cmd[COMMAND_PITCH] = stabilization_att_indi_cmd[COMMAND_PITCH]; + stabilization_cmd[COMMAND_YAW] = stabilization_att_indi_cmd[COMMAND_YAW]; + + /* bound the result */ + BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); + BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); + BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); +#else + stabilization_indi_calc_cmd(rate_ref, in_flight); +#endif } -void stabilization_indi_rate_run(bool in_flight __attribute__((unused)), struct Int32Rates rates_sp) +void stabilization_rate_run(bool in_flight __attribute__((unused)), struct Int32Rates rates_sp) { /* set rate set-point */ stabilization_rate_set_setpoint_i(rates_sp) // NOT SURE ABOUT DOING THIS! /* compute the INDI rate command */ stabilization_indi_rate_calc_cmd(indi_comands, in_flight); - - /* copy the INDI rate command */ - stabilization_cmd[COMMAND_ROLL] = indi_commands[COMMAND_ROLL]; - stabilization_cmd[COMMAND_PITCH] = indi_commands[COMMAND_PITCH]; - stabilization_cmd[COMMAND_YAW] = indi_commands[COMMAND_YAW]; - - /* bound the result */ - BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); - BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); - BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); } void stabilization_indi_rate_enter(void) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.h index e0e8694d6f9..55ea18064d4 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.h @@ -1,5 +1,6 @@ /* * Copyright (C) 2016 Freek van Tienen + * 2020 Rohan Chotalal * * This file is part of paparazzi. * @@ -26,13 +27,13 @@ #ifndef STABILIZATION_RATE_INDI #define STABILIZATION_RATE_INDI +/* access declarations of basic functions from "stabilization_rate.h" file */ +#include "firmwares/rotorcraft/stabilization/stabilization_rate.h" + #include "math/pprz_algebra_int.h" -extern void stabilization_rate_init(void); -extern void stabilization_rate_read_rc(void); -extern void stabilization_rate_read_rc_switched_sticks(void); extern void stabilization_indi_rate_set_setpoint_i(struct Int32Rates *pqr); -extern void stabilization_indi_rate_run(bool in_flight, struct Int32Rates rates_sp); +extern void stabilization_rate_run(bool in_flight, struct Int32Rates rates_sp); extern void stabilization_indi_rate_enter(void); extern struct FloatRates stabilization_rate_sp;