flight_altitude moved to nav

conf/settings/tuning.xml

@@ -5,7 +5,7 @@
 <settings>
   <dl_settings>
     <dl_settings NAME="flight params">
-      <dl_setting MAX="2000" MIN="0" STEP="10" VAR="v_ctl_flight_altitude" shortname="altitude"/>
+      <dl_setting MAX="2000" MIN="0" STEP="10" VAR="flight_altitude" shortname="altitude"/>
 <!--      <dl_setting MAX="2" MIN="0" STEP="1" VAR="light_mode"/> -->
       <dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_east"/>
       <dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="wind_north"/>

sw/airborne/fw_v_ctl.c

@@ -29,7 +29,6 @@
  */
 
 #include "fw_v_ctl.h"
-//#include "autopilot.h"
 #include "estimator.h"
 #include "nav.h"
 #include "airframe.h"
@@ -40,9 +39,6 @@ float v_ctl_altitude_pre_climb;
 float v_ctl_altitude_pgain;
 float v_ctl_altitude_error;
 
-/** Dynamically adjustable, reset to nav_altitude when it is changing */
-float v_ctl_flight_altitude;
-
 /* inner loop */
 float v_ctl_climb_setpoint;
 uint8_t v_ctl_climb_mode;
@@ -103,16 +99,7 @@ void v_ctl_init( void ) {
  * outer loop
  * \brief Computes v_ctl_climb_setpoint and sets v_ctl_auto_throttle_submode 
  */
-static float last_nav_altitude;
 void v_ctl_altitude_loop( void ) {
-  //mmmm LOOKATME : should that be in nav ???
-  if (nav_altitude != last_nav_altitude) {
-    v_ctl_flight_altitude = nav_altitude;
-    last_nav_altitude = nav_altitude;
-  }
-
-  v_ctl_altitude_setpoint = v_ctl_flight_altitude + altitude_shift; 
-
   v_ctl_altitude_error = estimator_z - v_ctl_altitude_setpoint;
   v_ctl_climb_setpoint = v_ctl_altitude_pgain * v_ctl_altitude_error
     + v_ctl_altitude_pre_climb;

sw/airborne/fw_v_ctl.h

@@ -38,7 +38,6 @@
 extern float v_ctl_altitude_setpoint;
 extern float v_ctl_altitude_pre_climb;
 extern float v_ctl_altitude_pgain;
-extern float v_ctl_flight_altitude;
 
 /* inner loop */
 extern float v_ctl_climb_setpoint;

sw/airborne/nav.c

@@ -68,6 +68,10 @@ int16_t segment_x_1, segment_y_1, segment_x_2, segment_y_2;
 uint8_t horizontal_mode;
 float circle_bank = 0;
 
+/** Dynamically adjustable, reset to nav_altitude when it is changing */
+float flight_altitude;
+
+
 #define PowerVoltage() (vsupply/10.)
 #define RcRoll(travel) (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
 
@@ -479,6 +483,13 @@ void nav_update(void) {
   if ( vertical_mode == VERTICAL_MODE_AUTO_CLIMB)
     v_ctl_auto_throttle_submode =  V_CTL_AUTO_THROTTLE_STANDARD;
 #endif
+
+  static float last_nav_altitude;
+  if (fabs(nav_altitude - last_nav_altitude) > 1.) {
+    flight_altitude = nav_altitude;
+    last_nav_altitude = nav_altitude;
+  }
+  v_ctl_altitude_setpoint = flight_altitude + altitude_shift;
 }
 
 

sw/airborne/nav.h

@@ -57,7 +57,7 @@ extern const uint8_t nb_waypoint;
 extern struct point waypoints[]; /** size == nb_waypoint + 1 */
 extern bool_t moved_waypoints[]; /** size == nb_waypoint + 1 */
 
-extern float desired_x, desired_y, altitude_shift, nav_altitude;
+extern float desired_x, desired_y, altitude_shift, nav_altitude, flight_altitude;
 
 extern uint16_t nav_desired_gaz;
 extern float nav_pitch, rc_pitch;