Long distance altitude control

conf/flight_plans/tudelft/nederdrone_valkenburg.xml

@@ -1,14 +1,15 @@
 <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
 
-<flight_plan alt="60" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="5000" name="Nederdrone Valkenburg" security_height="2">
+<flight_plan alt="60" ground_alt="0" lat0="52.168595" lon0="4.412444" max_dist_from_home="500000" name="Nederdrone Valkenburg" security_height="2">
   <header>
     #include "autopilot.h"
     #include "modules/datalink/datalink.h"
 </header>
   <waypoints>
     <waypoint name="HOME" x="12.6" y="-48.7"/>
     <waypoint name="CLIMB" x="62.6" y="-80.4"/>
-    <waypoint name="STDBY" x="106.0" y="-55.1"/>
+    <!-- <waypoint name="STDBY" x="106.0" y="-55.1"/> -->
+    <waypoint name="STDBY" lat="51.168595" lon="4.412444"/>
     <waypoint name="p1" x="322.0" y="-254.3"/>
     <waypoint name="p2" x="696.0" y="-148.1"/>
     <waypoint name="p3" x="600.4" y="73.4"/>
@@ -46,17 +47,17 @@
       <define name="RECTANGLE_SURVEY_DEFAULT_SWEEP" value="100"/>
     </module>
   </modules>
-  <exceptions>
+  <!-- <exceptions>
     <exception cond="Or(!InsideFlyzone(GetPosX(), GetPosY()), GetPosAlt() @GT GetAltRef() + 1500) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
     <exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
-  </exceptions>
+  </exceptions> -->
   <blocks>
     <block name="Wait GPS">
       <call_once fun="NavKillThrottle()"/>
       <while cond="!GpsFixValid() || !state.ned_initialized_i"/>
     </block>
     <block name="Geo init">
-      <while cond="LessThan(NavBlockTime(), 10)"/>
+      <!-- <while cond="LessThan(NavBlockTime(), 10)"/> -->
       <call_once fun="NavSetAltitudeReferenceHere()"/>
     </block>
     <block name="Holding point">

conf/userconf/tudelft/conf.xml

@@ -139,7 +139,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/cyclone_valkenburg.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/gps.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml"
    gui_color="#ffff7f7f0000"
   />
   <aircraft
@@ -280,9 +280,9 @@
    airframe="airframes/tudelft/nederdrone4.xml"
    radio="radios/crossfire_sbus.xml"
    telemetry="telemetry/highspeed_rotorcraft.xml"
-   flight_plan="flight_plans/tudelft/nederdrone_cyberzoo.xml"
+   flight_plan="flight_plans/tudelft/nederdrone_valkenburg.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml"
+   settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/nav_survey_rectangle_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml"
    gui_color="blue"
   />
   <aircraft

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c

@@ -526,7 +526,7 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
 #else
   float airspeed = stateGetAirspeed_f();
 #endif
-  struct NedCoor_f *groundspeed = stateGetSpeedNed_f();
+  struct NedCoor_f *groundspeed = stateGetSpeedLtp_f();
   struct FloatVect2 airspeed_v = { cpsi * airspeed, spsi * airspeed };
   struct FloatVect2 windspeed;
   VECT2_DIFF(windspeed, *groundspeed, airspeed_v);
@@ -583,13 +583,13 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
 
     accel_sp.x = cpsi * sp_accel_b.x - spsi * sp_accel_b.y;
     accel_sp.y = spsi * sp_accel_b.x + cpsi * sp_accel_b.y;
-    accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
+    accel_sp.z = (gi_speed_sp.z - stateGetSpeedLtp_f()->z) * gih_params.speed_gainz;
   }
   else { // Go somewhere in the shortest way
 
     if (airspeed > 10.f) {
       // Groundspeed vector in body frame
-      float groundspeed_x = cpsi * stateGetSpeedNed_f()->x + spsi * stateGetSpeedNed_f()->y;
+      float groundspeed_x = cpsi * stateGetSpeedLtp_f()->x + spsi * stateGetSpeedLtp_f()->y;
       float speed_increment = speed_sp_b_x - groundspeed_x;
 
       // limit groundspeed setpoint to max_airspeed + (diff gs and airspeed)
@@ -601,9 +601,9 @@ static struct FloatVect3 compute_accel_from_speed_sp(void)
     gi_speed_sp.x = cpsi * speed_sp_b_x - spsi * speed_sp_b_y;
     gi_speed_sp.y = spsi * speed_sp_b_x + cpsi * speed_sp_b_y;
 
-    accel_sp.x = (gi_speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain;
-    accel_sp.y = (gi_speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain;
-    accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz;
+    accel_sp.x = (gi_speed_sp.x - stateGetSpeedLtp_f()->x) * gih_params.speed_gain;
+    accel_sp.y = (gi_speed_sp.y - stateGetSpeedLtp_f()->y) * gih_params.speed_gain;
+    accel_sp.z = (gi_speed_sp.z - stateGetSpeedLtp_f()->z) * gih_params.speed_gainz;
   }
 
   // Bound the acceleration setpoint
@@ -653,30 +653,33 @@ struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struc
       pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
       gi_speed_sp.z = bound_vz_sp(pos_err.z * gih_params.pos_gainz + SPEED_FLOAT_OF_BFP(gv->zd_ref));
     } else if (v_mode == GUIDANCE_INDI_HYBRID_V_SPEED) {
-      gi_speed_sp.z = SPEED_FLOAT_OF_BFP(gv->zd_ref);
+      gi_speed_sp.z = -SPEED_FLOAT_OF_BFP(gv->zd_ref);
     } else {
       gi_speed_sp.z = 0.f;
     }
     accel_sp = compute_accel_from_speed_sp(); // compute accel sp
     if (v_mode == GUIDANCE_INDI_HYBRID_V_ACCEL) {
-      accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); // overwrite accel
+      accel_sp.z = (gi_speed_sp.z - stateGetSpeedLtp_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); // overwrite accel
     }
     return guidance_indi_run(&accel_sp, gh->sp.heading);
   }
   else if (h_mode == GUIDANCE_INDI_HYBRID_H_SPEED) {
     gi_speed_sp.x = SPEED_FLOAT_OF_BFP(gh->ref.speed.x);
     gi_speed_sp.y = SPEED_FLOAT_OF_BFP(gh->ref.speed.y);
     if (v_mode == GUIDANCE_INDI_HYBRID_V_POS) {
-      pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
+      // pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionNed_f()->z;
+      float hmsl_alt = state.ned_origin_f.hmsl - state.ned_origin_f.lla.alt;
+      pos_err.z = POS_FLOAT_OF_BFP(gv->z_ref) - stateGetPositionLla_f()->alt;
+      pos_err.z = -pos_err.z;
       gi_speed_sp.z = bound_vz_sp(pos_err.z * gih_params.pos_gainz + SPEED_FLOAT_OF_BFP(gv->zd_ref));
     } else if (v_mode == GUIDANCE_INDI_HYBRID_V_SPEED) {
-      gi_speed_sp.z = SPEED_FLOAT_OF_BFP(gv->zd_ref);
+      gi_speed_sp.z = -SPEED_FLOAT_OF_BFP(gv->zd_ref);
     } else {
       gi_speed_sp.z = 0.f;
     }
     accel_sp = compute_accel_from_speed_sp(); // compute accel sp
     if (v_mode == GUIDANCE_INDI_HYBRID_V_ACCEL) {
-      accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); // overwrite accel
+      accel_sp.z = (gi_speed_sp.z - stateGetSpeedLtp_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); // overwrite accel
     }
     return guidance_indi_run(&accel_sp, gh->sp.heading);
   }
@@ -693,10 +696,10 @@ struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struc
     }
     accel_sp = compute_accel_from_speed_sp(); // compute accel sp in case z control is required
     // overwrite accel X and Y
-    accel_sp.x = (gi_speed_sp.x - stateGetSpeedNed_f()->x) * gih_params.speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
-    accel_sp.y = (gi_speed_sp.y - stateGetSpeedNed_f()->y) * gih_params.speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
+    accel_sp.x = (gi_speed_sp.x - stateGetSpeedLtp_f()->x) * gih_params.speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.x);
+    accel_sp.y = (gi_speed_sp.y - stateGetSpeedLtp_f()->y) * gih_params.speed_gain + ACCEL_FLOAT_OF_BFP(gh->ref.accel.y);
     if (v_mode == GUIDANCE_INDI_HYBRID_V_ACCEL) {
-      accel_sp.z = (gi_speed_sp.z - stateGetSpeedNed_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); // overwrite accel
+      accel_sp.z = (gi_speed_sp.z - stateGetSpeedLtp_f()->z) * gih_params.speed_gainz + ACCEL_FLOAT_OF_BFP(gv->zdd_ref); // overwrite accel
     }
     return guidance_indi_run(&accel_sp, gh->sp.heading);
   }

sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c

@@ -236,13 +236,13 @@ void guidance_v_run(bool in_flight)
 
     case GUIDANCE_V_MODE_RC_CLIMB:
       guidance_v.zd_sp = guidance_v.rc_zd_sp;
-      gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
+      gv_update_ref_from_zd_sp(guidance_v.zd_sp, POS_BFP_OF_REAL(stateGetPositionLla_f()->alt));
       guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
       stabilization_cmd[COMMAND_THRUST] = guidance_v.delta_t;
       break;
 
     case GUIDANCE_V_MODE_CLIMB:
-      gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
+      gv_update_ref_from_zd_sp(guidance_v.zd_sp, POS_BFP_OF_REAL(stateGetPositionLla_f()->alt));
       guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
 #if !NO_RC_THRUST_LIMIT
       /* use rc limitation if available */
@@ -316,20 +316,21 @@ void guidance_v_update_ref(void)
 void guidance_v_from_nav(bool in_flight)
 {
   if (nav.vertical_mode == NAV_VERTICAL_MODE_ALT) {
-    guidance_v.z_sp = -POS_BFP_OF_REAL(nav.nav_altitude);
+    RunOnceEvery(30, printf("nav alt: %f\n", POS_BFP_OF_REAL(nav.nav_altitude)));
+    guidance_v.z_sp = POS_BFP_OF_REAL(nav.nav_altitude);
     guidance_v.zd_sp = 0;
     gv_update_ref_from_z_sp(guidance_v.z_sp);
     guidance_v_update_ref();
     guidance_v.delta_t = guidance_v_run_pos(in_flight, &guidance_v);
   } else if (nav.vertical_mode == NAV_VERTICAL_MODE_CLIMB) {
-    guidance_v.z_sp = stateGetPositionNed_i()->z;
-    guidance_v.zd_sp = -SPEED_BFP_OF_REAL(nav.climb);
-    gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
+    guidance_v.z_sp = POS_BFP_OF_REAL(stateGetPositionLla_f()->alt);
+    guidance_v.zd_sp = SPEED_BFP_OF_REAL(nav.climb);
+    gv_update_ref_from_zd_sp(guidance_v.zd_sp, POS_BFP_OF_REAL(stateGetPositionLla_f()->alt));
     guidance_v_update_ref();
     guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
   } else if (nav.vertical_mode == NAV_VERTICAL_MODE_MANUAL) {
-    guidance_v.z_sp = stateGetPositionNed_i()->z;
-    guidance_v.zd_sp = stateGetSpeedNed_i()->z;
+    guidance_v.z_sp = POS_BFP_OF_REAL(stateGetPositionLla_f()->alt);
+    guidance_v.zd_sp = SPEED_BFP_OF_REAL(stateGetSpeedLtp_f()->z);
     GuidanceVSetRef(guidance_v.z_sp, guidance_v.zd_sp, 0);
     guidance_v_run_enter();
     guidance_v.delta_t = nav.throttle;
@@ -368,7 +369,7 @@ void guidance_v_guided_run(bool in_flight)
       break;
     case GUIDANCE_V_GUIDED_MODE_CLIMB:
       // Climb
-      gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z);
+      gv_update_ref_from_zd_sp(guidance_v.zd_sp, POS_BFP_OF_REAL(stateGetPositionLla_f()->alt));
       guidance_v_update_ref();
       guidance_v.delta_t = guidance_v_run_speed(in_flight, &guidance_v);
       break;

sw/airborne/firmwares/rotorcraft/navigation.h

@@ -193,6 +193,39 @@ extern float flight_altitude; // hmsl flight altitude in meters
  */
 #define GetAltRef() (state.ned_origin_f.hmsl)
 
+static float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
+{
+	const double lat_now_rad = RadOfDeg(lat_now);
+	const double lat_next_rad = RadOfDeg(lat_next);
+
+	const double cos_lat_next = cos(lat_next_rad);
+	const double d_lon = RadOfDeg(lon_next - lon_now);
+
+	/* conscious mix of double and float trig function to maximize speed and efficiency */
+
+	const float y = (sin(d_lon) * cos_lat_next);
+	const float x = (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos_lat_next * cos(d_lon));
+
+  float a = atan2f(y, x);
+  FLOAT_ANGLE_NORMALIZE(a);
+
+	return a;
+}
+
+static void
+get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float *v_n, float *v_e)
+{
+  double CONSTANTS_RADIUS_OF_EARTH = 6371000.0;
+
+	const double lat_now_rad = RadOfDeg(lat_now);
+	const double lat_next_rad = RadOfDeg(lat_next);
+	const double d_lon = RadOfDeg(lon_next) - RadOfDeg(lon_now);
+
+	/* conscious mix of double and float trig function to maximize speed and efficiency */
+	*v_n = (CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)));
+	*v_e = (CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad));
+}
+
 
 extern void nav_init(void);
 extern void nav_run(void);
@@ -301,9 +334,49 @@ bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time);
 /*********** Navigation to  waypoint *************************************/
 static inline void NavGotoWaypoint(uint8_t wp)
 {
-  struct EnuCoor_f * _wp = waypoint_get_enu_f(wp);
+  // waypoint_get_lla
+  struct LlaCoor_i * _wp = waypoint_get_lla(wp);
+  struct LlaCoor_f _wp_f;
+  LLA_FLOAT_OF_BFP(_wp_f, *_wp);
+  // get ltp
+  struct LlaCoor_i * lla_curpos;
+  lla_curpos = stateGetPositionLla_i();
+  struct LlaCoor_f * lla_curpos_f;
+  lla_curpos_f = stateGetPositionLla_f();
+  // ltp_def_from_lla_i(&ltp_curpos, lla_curpos);
+  struct LtpDef_f *ltp_curpos;
+  ltp_curpos = stateGetLtp_f();
+  // lla to ltp (local ned)
+  // Convert lla to ned (in cm, no pos_frac!)
+  struct NedCoor_f waypoint_ltp;
+  ned_of_lla_point_f(&waypoint_ltp, ltp_curpos, &_wp_f);
+  // separate altitude
+  waypoint_ltp.z = -_wp_f.alt;
+
+  struct EnuCoor_f waypoint_ltp_enu;
+  waypoint_ltp_enu.x = waypoint_ltp.y;
+  waypoint_ltp_enu.y = waypoint_ltp.x;
+  waypoint_ltp_enu.z = -waypoint_ltp.z;
+
+  float v_n, v_e;
+  get_vector_to_next_waypoint(((double) lla_curpos->lat)/1.0e7, ((double) lla_curpos->lon)/1.0e7, ((double) _wp->lat)/1.0e7, ((double) _wp->lon)/1.0e7, &v_n, &v_e);
+
+  float bearing = get_bearing_to_next_waypoint(((double) lla_curpos->lat)/1.0e7, ((double) lla_curpos->lon)/1.0e7, ((double) _wp->lat)/1.0e7, ((double) _wp->lon)/1.0e7);
+
+  RunOnceEvery(30, printf("la lon wp: %f, %f, %f\n", _wp_f.lat, _wp_f.lon, _wp_f.alt));
+  RunOnceEvery(30, printf("ltp : %f, %f, %f\n", ltp_curpos->lla.lat, ltp_curpos->lla.lon, ltp_curpos->lla.alt));
+  RunOnceEvery(30, printf("pos sp enu: %f, %f, %f\n", waypoint_ltp_enu.x, waypoint_ltp_enu.y, waypoint_ltp_enu.z));
+  RunOnceEvery(30, printf("new func: %f, %f, %f\n", v_e, v_n, bearing));
+  double lat = ((double) lla_curpos->lat)/1.0e7;
+  double lon = ((double) lla_curpos->lon)/1.0e7;
+  RunOnceEvery(30, printf("la lon: %f, %f\n", lat, lon));
+  // double latwp = ((double) _wp->lat)/1.0e7;
+  // double lonwp = ((double) _wp->lon)/1.0e7;
+  RunOnceEvery(30, printf("la lon : %f, %f\n", lla_curpos_f->lat, lla_curpos_f->lon));
+
   if (_wp != NULL) {
-    nav.nav_goto(_wp);
+    // nav.nav_goto(_wp);
+    nav.nav_goto(&waypoint_ltp_enu);
   }
 }
 

sw/airborne/modules/nav/nav_rotorcraft_hybrid.c

@@ -67,7 +67,9 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
   // Calculate position error
   struct FloatVect2 pos_error;
   struct EnuCoor_f *pos = stateGetPositionEnu_f();
-  VECT2_DIFF(pos_error, nav.target, *pos);
+  // VECT2_DIFF(pos_error, nav.target, *pos);
+  VECT2_COPY(pos_error, nav.target);
+  RunOnceEvery(30, printf("pos_error: %f, %f\n", pos_error.x, pos_error.y));
 
   struct FloatVect2 speed_sp;
   VECT2_SMUL(speed_sp, pos_error, gih_params.pos_gain);
@@ -80,7 +82,7 @@ static void nav_hybrid_goto(struct EnuCoor_f *wp)
     // Calculate max speed when decelerating at MAX capacity a_max
     // distance travelled d = 1/2 a_max t^2
     // The time in which it does this is: T = V / a_max
-    // The maximum speed at which to fly to still allow arriving with zero 
+    // The maximum speed at which to fly to still allow arriving with zero
     // speed at the waypoint given maximum deceleration is: V = sqrt(2 * a_max * d)
     float max_speed_decel2 = fabsf(2.f * dist_to_wp * nav_max_deceleration_sp); // dist_to_wp can only be positive, but just in case
     float max_speed_decel = sqrtf(max_speed_decel2);

sw/airborne/modules/nav/waypoints.c

@@ -118,6 +118,14 @@ float waypoint_get_alt(uint8_t wp_id)
   return 0.f;
 }
 
+float waypoint_get_lla_alt(uint8_t wp_id)
+{
+  if (wp_id < nb_waypoint) {
+    return waypoints[wp_id].lla.alt/1000.f;
+  }
+  return 0.f;
+}
+
 float waypoint_get_lat_deg(uint8_t wp_id)
 {
   if (wp_id < nb_waypoint) {
@@ -335,9 +343,9 @@ void waypoint_localize(uint8_t wp_id)
     struct EnuCoor_i enu;
     enu_of_lla_point_i(&enu, &state.ned_origin_i, &waypoints[wp_id].lla);
     // convert ENU pos from cm to BFP with INT32_POS_FRAC
-    enu.x = POS_BFP_OF_REAL(enu.x) / 100;
-    enu.y = POS_BFP_OF_REAL(enu.y) / 100;
-    enu.z = POS_BFP_OF_REAL(enu.z) / 100;
+    enu.x = POS_BFP_OF_REAL((int64_t) enu.x) / 100;
+    enu.y = POS_BFP_OF_REAL((int64_t) enu.y) / 100;
+    enu.z = POS_BFP_OF_REAL((int64_t) enu.z) / 100;
     waypoints[wp_id].enu_i = enu;
     SetBit(waypoints[wp_id].flags, WP_FLAG_ENU_I);
     ENU_FLOAT_OF_BFP(waypoints[wp_id].enu_f, waypoints[wp_id].enu_i);

sw/airborne/modules/nav/waypoints.h

@@ -53,7 +53,7 @@ extern struct Waypoint waypoints[];
 /* aliases for backwards compatibilty */
 #define WaypointX(_wp)    waypoint_get_x(_wp)
 #define WaypointY(_wp)    waypoint_get_y(_wp)
-#define WaypointAlt(_wp)  waypoint_get_alt(_wp)
+#define WaypointAlt(_wp)  waypoint_get_lla_alt(_wp)
 #define Height(_h) (_h)
 
 extern void waypoints_init(void);
@@ -72,6 +72,7 @@ extern float waypoint_get_x(uint8_t wp_id);
 extern float waypoint_get_y(uint8_t wp_id);
 /** Get altitude of waypoint in meters (above reference) */
 extern float waypoint_get_alt(uint8_t wp_id);
+extern float waypoint_get_lla_alt(uint8_t wp_id);
 /** Get latitude of waypoint in deg */
 extern float waypoint_get_lat_deg(uint8_t wp_id);
 /** Get latitude of waypoint in rad */