Skip to content

Commit

Permalink
Merge pull request #986 from paparazzi/mission_module_latlon_int
Browse files Browse the repository at this point in the history 
[modules][messages] mission: LLA waypoints in 1e7deg instead of float

A 32bit float has 23bits for the mantissa, which is a bit more than 7 digits.
Since you already use 3 digits to represent the part to the left of the decimal point, that leaves about 4 digits for the rest..
So in the worst case of 180deg you have a resolution of only ~3m!

With 1e7deg 32bit int you get ~1cm resolution.

In the fixedwing version LLA is still converted to float first, since we seem to miss functions to convert LLA to UTM in int or double...
At least it is better for rotorcraft and same as before for fixedwings...
  • Loading branch information
@flixr
flixr committed Nov 26, 2014
2 parents c0074af + f15cd03 commit 1454b1a
Show file tree
Hide file tree
Showing 5 changed files with 77 additions and 70 deletions.
64 changes: 32 additions & 32 deletions conf/messages.xml
View file
Expand Up @@ -2319,16 +2319,16 @@
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
<field name="wp_east" type="float" unit="m"/>
<field name="wp_north" type="float" unit="m"/>
<field name="wp_alt" type="float" unit="m"/>
<field name="wp_alt" type="float" unit="m">altitude above geoid (MSL)</field>
<field name="duration" type="float" unit="s"/>
</message>

<message name="MISSION_GOTO_WP_LLA" id="21" link="forwarded">
<field name="ac_id" type="uint8"/>
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
<field name="wp_lat" type="float" unit="deg"/>
<field name="wp_lon" type="float" unit="deg"/>
<field name="wp_alt" type="float" unit="m"/>
<field name="wp_lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="wp_lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="wp_alt" type="int32" unit="mm" alt_unit="m">altitude above geoid (MSL)</field>
<field name="duration" type="float" unit="s"/>
</message>

Expand All @@ -2337,17 +2337,17 @@
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
<field name="center_east" type="float" unit="m"/>
<field name="center_north" type="float" unit="m"/>
<field name="center_alt" type="float" unit="m"/>
<field name="center_alt" type="float" unit="m">altitude above geoid (MSL)</field>
<field name="radius" type="float" unit="m"/>
<field name="duration" type="float" unit="s"/>
</message>

<message name="MISSION_CIRCLE_LLA" id="23" link="forwarded">
<field name="ac_id" type="uint8"/>
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
<field name="center_lat" type="float" unit="deg"/>
<field name="center_lon" type="float" unit="deg"/>
<field name="center_alt" type="float" unit="m"/>
<field name="center_lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="center_lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="center_alt" type="int32" unit="mm" alt_unit="m">altitude above geoid (MSL)</field>
<field name="radius" type="float" unit="m"/>
<field name="duration" type="float" unit="s"/>
</message>
Expand All @@ -2359,18 +2359,18 @@
<field name="segment_north_1" type="float" unit="m"/>
<field name="segment_east_2" type="float" unit="m"/>
<field name="segment_north_2" type="float" unit="m"/>
<field name="segment_alt" type="float" unit="m"/>
<field name="segment_alt" type="float" unit="m">altitude above geoid (MSL)</field>
<field name="duration" type="float" unit="s"/>
</message>

<message name="MISSION_SEGMENT_LLA" id="25" link="forwarded">
<field name="ac_id" type="uint8"/>
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
<field name="segment_lat_1" type="float" unit="deg"/>
<field name="segment_lon_1" type="float" unit="deg"/>
<field name="segment_lat_2" type="float" unit="deg"/>
<field name="segment_lon_2" type="float" unit="deg"/>
<field name="segment_alt" type="float" unit="m"/>
<field name="segment_lat_1" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="segment_lon_1" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="segment_lat_2" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="segment_lon_2" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="segment_alt" type="int32" unit="mm" alt_unit="m">altitude above geoid (MSL)</field>
<field name="duration" type="float" unit="s"/>
</message>

Expand All @@ -2387,25 +2387,25 @@
<field name="point_north_4" type="float" unit="m"/>
<field name="point_east_5" type="float" unit="m"/>
<field name="point_north_5" type="float" unit="m"/>
<field name="path_alt" type="float" unit="m"/>
<field name="path_alt" type="float" unit="m">altitude above geoid (MSL)</field>
<field name="duration" type="float" unit="s"/>
<field name="nb" type="uint8"/>
</message>

<message name="MISSION_PATH_LLA" id="27" link="forwarded">
<field name="ac_id" type="uint8"/>
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
<field name="point_lat_1" type="float" unit="deg"/>
<field name="point_lon_1" type="float" unit="deg"/>
<field name="point_lat_2" type="float" unit="deg"/>
<field name="point_lon_2" type="float" unit="deg"/>
<field name="point_lat_3" type="float" unit="deg"/>
<field name="point_lon_3" type="float" unit="deg"/>
<field name="point_lat_4" type="float" unit="deg"/>
<field name="point_lon_4" type="float" unit="deg"/>
<field name="point_lat_5" type="float" unit="deg"/>
<field name="point_lon_5" type="float" unit="deg"/>
<field name="path_alt" type="float" unit="m"/>
<field name="point_lat_1" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="point_lon_1" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="point_lat_2" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="point_lon_2" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="point_lat_3" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="point_lon_3" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="point_lat_4" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="point_lon_4" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="point_lat_5" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="point_lon_5" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="path_alt" type="int32" unit="mm" alt_unit="m">altitude above geoid (MSL)</field>
<field name="duration" type="float" unit="s"/>
<field name="nb" type="uint8"/>
</message>
Expand All @@ -2417,18 +2417,18 @@
<field name="survey_north_1" type="float" unit="m"/>
<field name="survey_east_2" type="float" unit="m"/>
<field name="survey_north_2" type="float" unit="m"/>
<field name="survey_alt" type="float" unit="m"/>
<field name="survey_alt" type="float" unit="m">altitude above geoid (MSL)</field>
<field name="duration" type="float" unit="s"/>
</message>

<message name="MISSION_SURVEY_LLA" id="29" link="forwarded">
<field name="ac_id" type="uint8"/>
<field name="insert" type="uint8" values="APPEND|PREPEND|REPLACE_CURRENT|REPLACE_ALL"/>
<field name="survey_lat_1" type="float" unit="deg"/>
<field name="survey_lon_1" type="float" unit="deg"/>
<field name="survey_lat_2" type="float" unit="deg"/>
<field name="survey_lon_2" type="float" unit="deg"/>
<field name="survey_alt" type="float" unit="m"/>
<field name="survey_lat_1" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="survey_lon_1" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="survey_lat_2" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="survey_lon_2" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="survey_alt" type="int32" unit="mm" alt_unit="m">altitude above geoid (MSL)</field>
<field name="duration" type="float" unit="s"/>
</message>

Expand Down
44 changes: 22 additions & 22 deletions sw/airborne/modules/mission/mission_common.c
View file
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
6 changes: 3 additions & 3 deletions sw/airborne/modules/mission/mission_common.h
View file
Expand Up @@ -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
*
Expand Down
10 changes: 7 additions & 3 deletions sw/airborne/modules/mission/mission_fw_nav.c
View file
Expand Up @@ -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
Expand Down
23 changes: 13 additions & 10 deletions sw/airborne/modules/mission/mission_rotorcraft_nav.c
View file
Expand Up @@ -37,23 +37,26 @@
//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) {
// 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;
/// 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 system
if (!state.ned_initialized_i)
return FALSE;

// change geoid alt to ellipsoid alt
lla->alt = lla->alt - state.ned_origin_f.hmsl + state.ned_origin_f.lla.alt;
lla->alt = lla->alt - state.ned_origin_i.hmsl + state.ned_origin_i.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);
Expand All @@ -63,7 +66,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;
}
Expand Down

0 comments on commit 1454b1a

Please sign in to comment.