[state interface] fw control/guidance/nav converted

Only the basic navigation routines are converted to the new state interface Some stuff are missing into the state interface: angle of attack and sideslip angle
paparazzi · Jul 25, 2012 · aa2cd51 · aa2cd51
1 parent 242c6e2
commit aa2cd51
Show file tree

Hide file tree

Showing 8 changed files with 75 additions and 63 deletions.
diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v.c
@@ -26,7 +26,7 @@
  */
 
 #include "firmwares/fixedwing/guidance/guidance_v.h"
-#include "estimator.h"
+#include "state.h"
 #include "subsystems/nav.h"
 #include "generated/airframe.h"
 #include "firmwares/fixedwing/autopilot.h"
@@ -207,7 +207,7 @@ void v_ctl_altitude_loop( void ) {
   }
 #endif
 
-  v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z;
+  v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
   v_ctl_climb_setpoint = altitude_pgain_boost * v_ctl_altitude_pgain * v_ctl_altitude_error
     + v_ctl_altitude_pre_climb * v_ctl_altitude_pre_climb_correction;
   BoundAbs(v_ctl_climb_setpoint, v_ctl_altitude_max_climb);
@@ -253,7 +253,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
   static float last_err;
 
   float f_throttle = 0;
-  float err  = estimator_z_dot - v_ctl_climb_setpoint;
+  float err  = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
   float d_err = err - last_err;
   last_err = err;
   float controlled_throttle = v_ctl_auto_throttle_cruise_throttle
@@ -326,14 +326,14 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
   v_ctl_climb_setpoint_last = v_ctl_climb_setpoint;
 
   // Pitch control (input: rate of climb error, output: pitch setpoint)
-  float err  = estimator_z_dot - v_ctl_climb_setpoint;
+  float err  = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
   v_ctl_auto_pitch_sum_err += err;
   BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);
   v_ctl_pitch_of_vz = -v_ctl_auto_pitch_pgain *
     (err + v_ctl_auto_pitch_igain * v_ctl_auto_pitch_sum_err);
 
   // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
-  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod);
+  float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f());
   v_ctl_auto_groundspeed_sum_err += err_groundspeed;
   BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
   v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) * v_ctl_auto_groundspeed_pgain;
@@ -345,7 +345,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
   }
 
   // Airspeed control loop (input: airspeed controlled, output: throttle controlled)
-  float err_airspeed = (v_ctl_auto_airspeed_controlled - estimator_airspeed);
+  float err_airspeed = (v_ctl_auto_airspeed_controlled - *stateGetAirspeed_f());
   v_ctl_auto_airspeed_sum_err += err_airspeed;
   BoundAbs(v_ctl_auto_airspeed_sum_err, V_CTL_AUTO_AIRSPEED_MAX_SUM_ERR);
   controlled_throttle = (err_airspeed + v_ctl_auto_airspeed_sum_err * v_ctl_auto_airspeed_igain) * v_ctl_auto_airspeed_pgain;
@@ -367,7 +367,7 @@ inline static void v_ctl_climb_auto_throttle_loop(void) {
  */
 #ifdef V_CTL_AUTO_PITCH_PGAIN
 inline static void v_ctl_climb_auto_pitch_loop(void) {
-  float err  = estimator_z_dot - v_ctl_climb_setpoint;
+  float err  = stateGetSpeedEnu_f()->z - v_ctl_climb_setpoint;
   v_ctl_throttle_setpoint = nav_throttle_setpoint;
   v_ctl_auto_pitch_sum_err += err;
   BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR);

diff --git a/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c b/sw/airborne/firmwares/fixedwing/guidance/guidance_v_n.c
@@ -27,7 +27,7 @@
 
 #include "firmwares/fixedwing/guidance/guidance_v.h"
 #include "firmwares/fixedwing/guidance/guidance_v_n.h"
-#include "estimator.h"
+#include "state.h"
 #include "subsystems/nav.h"
 #include "generated/airframe.h"
 #include "firmwares/fixedwing/autopilot.h"
@@ -185,7 +185,7 @@ void v_ctl_altitude_loop( void ) {
   //static float last_lead_input = 0.;
 
   // Altitude error
-  v_ctl_altitude_error = v_ctl_altitude_setpoint - estimator_z;
+  v_ctl_altitude_error = v_ctl_altitude_setpoint - stateGetPositionUtm_f()->alt;
   v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error + v_ctl_altitude_pre_climb;
 
   // Lead controller
@@ -209,7 +209,7 @@ static inline void v_ctl_set_pitch ( void ) {
     v_ctl_auto_pitch_sum_err = 0;
 
   // Compute errors
-  float err = v_ctl_climb_setpoint - estimator_z_dot;
+  float err = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
   float d_err = err - last_err;
   last_err = err;
 
@@ -234,7 +234,7 @@ static inline void v_ctl_set_throttle( void ) {
     v_ctl_auto_throttle_sum_err = 0;
 
   // Compute errors
-  float err =  v_ctl_climb_setpoint - estimator_z_dot;
+  float err =  v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
   float d_err = err - last_err;
   last_err = err;
 
@@ -264,7 +264,7 @@ static inline void v_ctl_set_airspeed( void ) {
   Bound(v_ctl_auto_airspeed_setpoint, V_CTL_AIRSPEED_MIN, V_CTL_AIRSPEED_MAX);
 
   // Compute errors
-  float err_vz = v_ctl_climb_setpoint - estimator_z_dot;
+  float err_vz = v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z;
   float d_err_vz = (err_vz - last_err_vz)*AIRSPEED_LOOP_PERIOD;
   last_err_vz = err_vz;
   if (v_ctl_auto_throttle_igain > 0.) {
@@ -276,7 +276,7 @@ static inline void v_ctl_set_airspeed( void ) {
     BoundAbs(v_ctl_auto_pitch_sum_err, V_CTL_AUTO_PITCH_MAX_SUM_ERR / v_ctl_auto_pitch_igain);
   }
 
-  float err_airspeed = v_ctl_auto_airspeed_setpoint - estimator_airspeed;
+  float err_airspeed = v_ctl_auto_airspeed_setpoint - *stateGetAirspeed_f();
   float d_err_airspeed = (err_airspeed - last_err_as)*AIRSPEED_LOOP_PERIOD;
   last_err_as = err_airspeed;
   if (v_ctl_auto_airspeed_throttle_igain > 0.) {
@@ -321,7 +321,7 @@ static inline void v_ctl_set_airspeed( void ) {
 
 static inline void v_ctl_set_groundspeed( void ) {
   // Ground speed control loop (input: groundspeed error, output: airspeed controlled)
-  float err_groundspeed = v_ctl_auto_groundspeed_setpoint - estimator_hspeed_mod;
+  float err_groundspeed = v_ctl_auto_groundspeed_setpoint - *stateGetHorizontalSpeedNorm_f();
   v_ctl_auto_groundspeed_sum_err += err_groundspeed;
   BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
   v_ctl_auto_airspeed_setpoint = err_groundspeed * v_ctl_auto_groundspeed_pgain + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain;

diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_adaptive.c
@@ -32,7 +32,7 @@
 #include "led.h"
 #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
 #include "firmwares/fixedwing/stabilization/stabilization_adaptive.h"
-#include "estimator.h"
+#include "state.h"
 #include "subsystems/nav.h"
 #include "generated/airframe.h"
 #include "firmwares/fixedwing/guidance/guidance_v.h"
@@ -205,14 +205,14 @@ void h_ctl_course_loop ( void ) {
   static float last_err;
 
   // Ground path error
-  float err = h_ctl_course_setpoint - estimator_hspeed_dir;
+  float err = h_ctl_course_setpoint - (*stateGetHorizontalSpeedDir_f());
   NormRadAngle(err);
 
   float d_err = err - last_err;
   last_err = err;
   NormRadAngle(d_err);
 
-  float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
+  float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
   Bound(speed_depend_nav, 0.66, 1.5);
 
   h_ctl_roll_setpoint = h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank
@@ -227,7 +227,7 @@ static inline void compute_airspeed_ratio( void ) {
   if (use_airspeed_ratio) {
     // low pass airspeed
     static float airspeed = 0.;
-    airspeed = ( 15*airspeed + estimator_airspeed ) / 16;
+    airspeed = ( 15*airspeed + (*stateGetAirspeed_f()) ) / 16;
     // compute ratio
     float airspeed_ratio = airspeed / NOMINAL_AIRSPEED;
     Bound(airspeed_ratio, AIRSPEED_RATIO_MIN, AIRSPEED_RATIO_MAX);
@@ -289,13 +289,14 @@ inline static void h_ctl_roll_loop( void ) {
 #endif
 
   // Compute errors
-  float err = h_ctl_ref_roll_angle - estimator_phi;
+  float err = h_ctl_ref_roll_angle - stateGetNedToBodyEulers_f()->phi;
+  struct FloatRates* body_rate = stateGetBodyRates_f();
 #ifdef SITL
   static float last_err = 0;
-  estimator_p = (err - last_err)/(1/60.);
+  body_rate->p = (err - last_err)/(1/60.); // FIXME should be done in ahrs sim
   last_err = err;
 #endif
-  float d_err = h_ctl_ref_roll_rate - estimator_p;
+  float d_err = h_ctl_ref_roll_rate - body_rate->p;
 
   if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
     h_ctl_roll_sum_err = 0.;
@@ -329,7 +330,7 @@ inline static void loiter(void) {
   float pitch_trim;
 
 #if USE_AIRSPEED
-  if (estimator_airspeed > NOMINAL_AIRSPEED) {
+  if (stateGetAirspeed_f() > NOMINAL_AIRSPEED) {
     pitch_trim = v_ctl_pitch_dash_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MAX * AIRSPEED_RATIO_MAX) - 1);
   } else {
     pitch_trim = v_ctl_pitch_loiter_trim * (airspeed_ratio2-1) / ((AIRSPEED_RATIO_MIN * AIRSPEED_RATIO_MIN) - 1);
@@ -359,7 +360,7 @@ inline static void h_ctl_pitch_loop( void ) {
   if (h_ctl_pitch_of_roll <0.)
     h_ctl_pitch_of_roll = 0.;
 
-  h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(estimator_phi);
+  h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi);
 #if USE_PITCH_TRIM
   loiter();
 #endif
@@ -386,9 +387,9 @@ inline static void h_ctl_pitch_loop( void ) {
 #endif
 
   // Compute errors
-  float err =  h_ctl_ref_pitch_angle - estimator_theta;
+  float err =  h_ctl_ref_pitch_angle - stateGetNedToBodyEulers_f()->theta;
 #if USE_GYRO_PITCH_RATE
-  float d_err = h_ctl_ref_pitch_rate - estimator_q;
+  float d_err = h_ctl_ref_pitch_rate - stateGetBodyRates_f()->q;
 #else // soft derivation
   float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref_pitch_rate;
   last_err = err;

diff --git a/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c b/sw/airborne/firmwares/fixedwing/stabilization/stabilization_attitude.c
@@ -31,7 +31,7 @@
 #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
 #include "std.h"
 #include "led.h"
-#include "estimator.h"
+#include "state.h"
 #include "subsystems/nav.h"
 #include "generated/airframe.h"
 #include "firmwares/fixedwing/guidance/guidance_v.h"
@@ -181,17 +181,17 @@ void h_ctl_course_loop ( void ) {
   static float last_err;
 
   // Ground path error
-  float err = estimator_hspeed_dir - h_ctl_course_setpoint;
+  float err = *stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
   NormRadAngle(err);
 
 #ifdef STRONG_WIND
   // Usefull path speed
   const float reference_advance = (NOMINAL_AIRSPEED / 2.);
-  float advance = cos(err) * estimator_hspeed_mod / reference_advance;
+  float advance = cos(err) * (*stateGetHorizontalSpeedNorm_f()) / reference_advance;
 
   if (
        (advance < 1.)  &&                          // Path speed is small
-       (estimator_hspeed_mod < reference_advance)  // Small path speed is due to wind (small groundspeed)
+       ((*stateGetHorizontalSpeedNorm_f()) < reference_advance)  // Small path speed is due to wind (small groundspeed)
      )
   {
 /*
@@ -210,7 +210,7 @@ void h_ctl_course_loop ( void ) {
 */
 
     // Heading error
-    float herr = estimator_psi - h_ctl_course_setpoint; //+crab);
+    float herr = *stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
     NormRadAngle(herr);
 
     if (advance < -0.5)              //<! moving in the wrong direction / big > 90 degree turn
@@ -268,7 +268,7 @@ void h_ctl_course_loop ( void ) {
   }
 #endif
 
-  float speed_depend_nav = estimator_hspeed_mod/NOMINAL_AIRSPEED;
+  float speed_depend_nav = (*stateGetHorizontalSpeedNorm_f())/NOMINAL_AIRSPEED;
   Bound(speed_depend_nav, 0.66, 1.5);
 
   float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
@@ -316,14 +316,15 @@ void h_ctl_attitude_loop ( void ) {
 
 #ifdef H_CTL_ROLL_ATTITUDE_GAIN
 inline static void h_ctl_roll_loop( void ) {
-  float err = estimator_phi - h_ctl_roll_setpoint;
+  float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint;
+  struct FloatRates* body_rate = stateGetBodyRates_f();
 #ifdef SITL
   static float last_err = 0;
-  estimator_p = (err - last_err)/(1/60.);
+  body_rate->p = (err - last_err)/(1/60.);
   last_err = err;
 #endif
   float cmd = h_ctl_roll_attitude_gain * err
-    + h_ctl_roll_rate_gain * estimator_p
+    + h_ctl_roll_rate_gain * body_rate->p
     + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
 
   h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
@@ -333,7 +334,7 @@ inline static void h_ctl_roll_loop( void ) {
 
 /** Computes h_ctl_aileron_setpoint from h_ctl_roll_setpoint */
 inline static void h_ctl_roll_loop( void ) {
-  float err = estimator_phi - h_ctl_roll_setpoint;
+  float err = stateGetNedToBodyEulers_f()->phi - h_ctl_roll_setpoint;
   float cmd = h_ctl_roll_pgain * err
     + v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
   h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
@@ -357,7 +358,7 @@ inline static void h_ctl_roll_loop( void ) {
 #ifdef H_CTL_RATE_LOOP
 
 static inline void h_ctl_roll_rate_loop() {
-  float err = estimator_p - h_ctl_roll_rate_setpoint;
+  float err = stateGetBodyRates_f()->p - h_ctl_roll_rate_setpoint;
 
   /* I term calculation */
   static float roll_rate_sum_err = 0.;
@@ -419,28 +420,29 @@ inline static float loiter(void) {
 
 inline static void h_ctl_pitch_loop( void ) {
   static float last_err;
+  struct FloatEulers* att = stateGetNedToBodyEulers_f();
   /* sanity check */
   if (h_ctl_elevator_of_roll <0.)
     h_ctl_elevator_of_roll = 0.;
 
-  h_ctl_pitch_loop_setpoint =  h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(estimator_phi);
+  h_ctl_pitch_loop_setpoint =  h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi);
 
 	float err = 0;
 
 #ifdef USE_AOA
 	switch (h_ctl_pitch_mode){
 		case H_CTL_PITCH_MODE_THETA:
-			err = estimator_theta - h_ctl_pitch_loop_setpoint;
+			err = att->theta - h_ctl_pitch_loop_setpoint;
 		break;
 		case H_CTL_PITCH_MODE_AOA:
-			err = estimator_AOA - h_ctl_pitch_loop_setpoint;
+			err = estimator_AOA - h_ctl_pitch_loop_setpoint; //TODO AOA into state interface
 		break;
 		default:
-			err = estimator_theta - h_ctl_pitch_loop_setpoint;
+			err = att->theta - h_ctl_pitch_loop_setpoint;
 		break;
 	}
 #else //NO_AOA
-	err = estimator_theta - h_ctl_pitch_loop_setpoint;
+	err = att->theta - h_ctl_pitch_loop_setpoint;
 #endif