diff --git a/conf/messages.xml b/conf/messages.xml
index de0cf7ac82d..545fed42136 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -2324,9 +2324,9 @@
-
-
-
+
+
+
@@ -2343,9 +2343,9 @@
-
-
-
+
+
+
@@ -2364,11 +2364,11 @@
-
-
-
-
-
+
+
+
+
+
@@ -2393,17 +2393,17 @@
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
@@ -2422,11 +2422,11 @@
-
-
-
-
-
+
+
+
+
+
diff --git a/sw/airborne/modules/mission/mission_common.c b/sw/airborne/modules/mission/mission_common.c
index 09523dbd47d..eea9810da3d 100644
--- a/sw/airborne/modules/mission/mission_common.c
+++ b/sw/airborne/modules/mission/mission_common.c
@@ -137,9 +137,9 @@ int mission_parse_GOTO_WP(void) {
int mission_parse_GOTO_WP_LLA(void) {
if (DL_MISSION_GOTO_WP_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
- struct LlaCoor_f lla;
- lla.lat = RadOfDeg(DL_MISSION_GOTO_WP_LLA_wp_lat(dl_buffer));
- lla.lon = RadOfDeg(DL_MISSION_GOTO_WP_LLA_wp_lon(dl_buffer));
+ struct LlaCoor_i lla;
+ lla.lat = DL_MISSION_GOTO_WP_LLA_wp_lat(dl_buffer);
+ lla.lon = DL_MISSION_GOTO_WP_LLA_wp_lon(dl_buffer);
lla.alt = DL_MISSION_GOTO_WP_LLA_wp_alt(dl_buffer);
struct _mission_element me;
@@ -172,9 +172,9 @@ int mission_parse_CIRCLE(void) {
int mission_parse_CIRCLE_LLA(void) {
if (DL_MISSION_CIRCLE_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
- struct LlaCoor_f lla;
- lla.lat = RadOfDeg(DL_MISSION_CIRCLE_LLA_center_lat(dl_buffer));
- lla.lon = RadOfDeg(DL_MISSION_CIRCLE_LLA_center_lon(dl_buffer));
+ struct LlaCoor_i lla;
+ lla.lat = DL_MISSION_CIRCLE_LLA_center_lat(dl_buffer);
+ lla.lon = DL_MISSION_CIRCLE_LLA_center_lon(dl_buffer);
lla.alt = DL_MISSION_CIRCLE_LLA_center_alt(dl_buffer);
struct _mission_element me;
@@ -210,12 +210,12 @@ int mission_parse_SEGMENT(void) {
int mission_parse_SEGMENT_LLA(void) {
if (DL_MISSION_SEGMENT_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
- struct LlaCoor_f from_lla, to_lla;
- from_lla.lat = RadOfDeg(DL_MISSION_SEGMENT_LLA_segment_lat_1(dl_buffer));
- from_lla.lon = RadOfDeg(DL_MISSION_SEGMENT_LLA_segment_lon_1(dl_buffer));
+ struct LlaCoor_i from_lla, to_lla;
+ from_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_1(dl_buffer);
+ from_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_1(dl_buffer);
from_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(dl_buffer);
- to_lla.lat = RadOfDeg(DL_MISSION_SEGMENT_LLA_segment_lat_2(dl_buffer));
- to_lla.lon = RadOfDeg(DL_MISSION_SEGMENT_LLA_segment_lon_2(dl_buffer));
+ to_lla.lat = DL_MISSION_SEGMENT_LLA_segment_lat_2(dl_buffer);
+ to_lla.lon = DL_MISSION_SEGMENT_LLA_segment_lon_2(dl_buffer);
to_lla.alt = DL_MISSION_SEGMENT_LLA_segment_alt(dl_buffer);
struct _mission_element me;
@@ -263,21 +263,21 @@ int mission_parse_PATH(void) {
int mission_parse_PATH_LLA(void) {
if (DL_MISSION_PATH_LLA_ac_id(dl_buffer) != AC_ID) return FALSE; // not for this aircraft
- struct LlaCoor_f lla[MISSION_PATH_NB];
- lla[0].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_1(dl_buffer));
- lla[0].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_1(dl_buffer));
+ struct LlaCoor_i lla[MISSION_PATH_NB];
+ lla[0].lat = DL_MISSION_PATH_LLA_point_lat_1(dl_buffer);
+ lla[0].lon = DL_MISSION_PATH_LLA_point_lon_1(dl_buffer);
lla[0].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
- lla[1].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_2(dl_buffer));
- lla[1].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_2(dl_buffer));
+ lla[1].lat = DL_MISSION_PATH_LLA_point_lat_2(dl_buffer);
+ lla[1].lon = DL_MISSION_PATH_LLA_point_lon_2(dl_buffer);
lla[1].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
- lla[2].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_3(dl_buffer));
- lla[2].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_3(dl_buffer));
+ lla[2].lat = DL_MISSION_PATH_LLA_point_lat_3(dl_buffer);
+ lla[2].lon = DL_MISSION_PATH_LLA_point_lon_3(dl_buffer);
lla[2].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
- lla[3].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_4(dl_buffer));
- lla[3].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_4(dl_buffer));
+ lla[3].lat = DL_MISSION_PATH_LLA_point_lat_4(dl_buffer);
+ lla[3].lon = DL_MISSION_PATH_LLA_point_lon_4(dl_buffer);
lla[3].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
- lla[4].lat = RadOfDeg(DL_MISSION_PATH_LLA_point_lat_5(dl_buffer));
- lla[4].lon = RadOfDeg(DL_MISSION_PATH_LLA_point_lon_5(dl_buffer));
+ lla[4].lat = DL_MISSION_PATH_LLA_point_lat_5(dl_buffer);
+ lla[4].lon = DL_MISSION_PATH_LLA_point_lon_5(dl_buffer);
lla[4].alt = DL_MISSION_PATH_LLA_path_alt(dl_buffer);
struct _mission_element me;
diff --git a/sw/airborne/modules/mission/mission_common.h b/sw/airborne/modules/mission/mission_common.h
index b76d5c89b10..4b1cec706d8 100644
--- a/sw/airborne/modules/mission/mission_common.h
+++ b/sw/airborne/modules/mission/mission_common.h
@@ -142,11 +142,11 @@ extern struct _mission_element * mission_get(void);
/** Get the ENU component of LLA mission point
* This function is firmware specific.
- * @param point pointer to the output ENU point
- * @param lla pointer to the input LLA coordinate
+ * @param point pointer to the output ENU point (float)
+ * @param lla pointer to the input LLA coordinates (int)
* @return TRUE if conversion is succesful, FALSE otherwise
*/
-extern bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla);
+extern bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla);
/** Run mission
*
diff --git a/sw/airborne/modules/mission/mission_fw_nav.c b/sw/airborne/modules/mission/mission_fw_nav.c
index 0fdd296f733..d697beb615c 100644
--- a/sw/airborne/modules/mission/mission_fw_nav.c
+++ b/sw/airborne/modules/mission/mission_fw_nav.c
@@ -34,12 +34,16 @@
#include "subsystems/navigation/common_nav.h"
#include "generated/flight_plan.h"
-// Utility function: converts lla to local point
-bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) {
+/// Utility function: converts lla (int) to local point (float)
+bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) {
+ /// TODO: don't convert to float, either use double or do completely in fixed point
+ struct LlaCoor_f lla_f;
+ LLA_FLOAT_OF_BFP(lla_f, *lla);
+
/* Computes from (lat, long) in the referenced UTM zone */
struct UtmCoor_f utm;
utm.zone = nav_utm_zone0;
- utm_of_lla_f(&utm, lla);
+ utm_of_lla_f(&utm, &lla_f);
/* Computes relative position to HOME waypoint
* and bound the distance to max_dist_from_home
diff --git a/sw/airborne/modules/mission/mission_rotorcraft_nav.c b/sw/airborne/modules/mission/mission_rotorcraft_nav.c
index 3a324efbca9..7f259a46802 100644
--- a/sw/airborne/modules/mission/mission_rotorcraft_nav.c
+++ b/sw/airborne/modules/mission/mission_rotorcraft_nav.c
@@ -37,23 +37,23 @@
//Buffer zone in [m] before MAX_DIST_FROM_HOME
#define BUFFER_ZONE_DIST 10
-// Utility function: converts lla to local point
-bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) {
+/// Utility function: converts lla (int) to local point (float)
+bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_i *lla) {
// return FALSE if there is no valid local coordinate
// FIXME we should only test if local frame is initialized, not valid
if (!stateIsLocalCoordinateValid()) return FALSE;
- // change geoid alt to ellipsoid alt
- lla->alt = lla->alt - state.ned_origin_f.hmsl + state.ned_origin_f.lla.alt;
//Compute ENU components from LLA with respect to ltp origin
- struct EnuCoor_f tmp_enu_point;
- enu_of_lla_point_f(&tmp_enu_point, &state.ned_origin_f, lla);
+ struct EnuCoor_i tmp_enu_point_i;
+ enu_of_lla_point_i(&tmp_enu_point_i, &state.ned_origin_i, lla);
+ struct EnuCoor_f tmp_enu_point_f;
+ ENU_FLOAT_OF_BFP(tmp_enu_point_f, tmp_enu_point_i);
//Bound the new waypoint with max distance from home
struct EnuCoor_f home;
ENU_FLOAT_OF_BFP(home, waypoints[WP_HOME]);
struct FloatVect2 vect_from_home;
- VECT2_DIFF(vect_from_home, tmp_enu_point, home);
+ VECT2_DIFF(vect_from_home, tmp_enu_point_f, home);
//Saturate the mission wp not to overflow max_dist_from_home
//including a buffer zone before limits
float dist_to_home = float_vect2_norm(&vect_from_home);
@@ -63,7 +63,7 @@ bool_t mission_point_of_lla(struct EnuCoor_f *point, struct LlaCoor_f *lla) {
}
// set new point
VECT2_SUM(*point, home, vect_from_home);
- point->z = tmp_enu_point.z;
+ point->z = tmp_enu_point_f.z;
return TRUE;
}