Added changes asked by Gautier

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);

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 */

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];

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);
 

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c

@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2016 Freek van Tienen <freek.v.tienen@gmail.com>
+ *               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)

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.h

@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2016 Freek van Tienen <freek.v.tienen@gmail.com>
+ *               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;