Skip to content

Commit

Permalink
[modules][messages] mission: LLA waypoints in 1e7deg instead of float
Browse files Browse the repository at this point in the history 
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.

Rotorcraft part: Also removed the removed the altitude conversion from geoid alt to ellipsoid alt,
requiring the alt in the message to already have ellipsoid alt.

addresses first part of #984
  • Loading branch information
@flixr
flixr committed Nov 26, 2014
1 parent 1e21302 commit bc342da
Show file tree
Hide file tree
Showing 5 changed files with 67 additions and 63 deletions.
54 changes: 27 additions & 27 deletions conf/messages.xml
View file
Expand Up @@ -2324,9 +2324,9 @@
<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"/>
<field name="duration" type="float" unit="s"/>
</message>

Expand All @@ -2343,9 +2343,9 @@
<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"/>
<field name="radius" type="float" unit="m"/>
<field name="duration" type="float" unit="s"/>
</message>
Expand All @@ -2364,11 +2364,11 @@
<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"/>
<field name="duration" type="float" unit="s"/>
</message>

Expand All @@ -2393,17 +2393,17 @@
<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"/>
<field name="duration" type="float" unit="s"/>
<field name="nb" type="uint8"/>
</message>
Expand All @@ -2422,11 +2422,11 @@
<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"/>
<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
16 changes: 8 additions & 8 deletions sw/airborne/modules/mission/mission_rotorcraft_nav.c
View file
Expand Up @@ -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);
Expand All @@ -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;
}
Expand Down

0 comments on commit bc342da

Please sign in to comment.