Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 01:53:48 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[modules][messages] mission: LLA waypoints in 1e7deg instead of float

Browse Source 
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.

addresses first part of #984 (at least for the rotorcraft version, fixedwing version still converts to LLA float in between...)
This commit is contained in:
Felix Ruess
2014-11-26 15:43:27 +01:00
parent 0fdbe7df4d
commit c24b395589
5 changed files with 74 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+32 -32
View File
@@ -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>
@@ -2337,7 +2337,7 @@
<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>
@@ -2345,9 +2345,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">altitude above geoid (MSL)</field>
<field name="radius" type="float" unit="m"/>
<field name="duration" type="float" unit="s"/>
</message>
@@ -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>
@@ -2387,7 +2387,7 @@
<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>
@@ -2395,17 +2395,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">altitude above geoid (MSL)</field>
<field name="duration" type="float" unit="s"/>
<field name="nb" type="uint8"/>
</message>
@@ -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>
sw/airborne/modules/mission/mission_common.c
+22 -22
View File
@@ -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;
sw/airborne/modules/mission/mission_common.h
+3 -3
View File
@@ -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
*
sw/airborne/modules/mission/mission_fw_nav.c
+7 -3
View File
@@ -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
sw/airborne/modules/mission/mission_rotorcraft_nav.c
+10 -7
View File
@@ -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) {
/// 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;
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);
@@ -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;
}