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

[gps] fix fixedwing UTM position usage

Browse Source 
- was correctly used for init but not normal reading
- add convenience function to get utm from GPS struct

fix stuff missed in #1476
This commit is contained in:
Gautier Hattenberger
2015-12-27 11:24:30 +01:00
committed by Felix Ruess
parent a1a8941758
commit 612a4f05e0
10 changed files with 58 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/cam_control/point.c
+1 -1
View File
@@ -408,7 +408,7 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
struct UtmCoor_f utm;
utm.east = nav_utm_east0 + fObjectEast;
utm.north = nav_utm_north0 + fObjectNorth;
utm.zone = gps.utm_pos.zone;
utm.zone = nav_utm_zone0;
struct LlaCoor_f lla;
lla_of_utm_f(&lla, &utm);
cam_point_lon = lla.lon * (180 / M_PI);
sw/airborne/modules/ins/ins_xsens.c
+1 -4
View File
@@ -275,10 +275,7 @@ void ins_xsens_register(void)
void ins_xsens_update_gps(struct GpsState *gps_s)
{
struct UtmCoor_f utm;
utm.east = gps_s->utm_pos.east / 100.;
utm.north = gps_s->utm_pos.north / 100.;
utm.zone = nav_utm_zone0;
struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
utm.alt = gps_s->hmsl / 1000.;
// set position
sw/airborne/modules/ins/ins_xsens700.c
+1 -4
View File
@@ -206,10 +206,7 @@ void ins_xsens_register(void)
void ins_xsens_update_gps(struct GpsState *gps_s)
{
struct UtmCoor_f utm;
utm.east = gps_s->utm_pos.east / 100.;
utm.north = gps_s->utm_pos.north / 100.;
utm.zone = nav_utm_zone0;
struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
utm.alt = gps_s->hmsl / 1000.;
// set position
sw/airborne/modules/meteo/meteo_france_DAQ.c
+5 -3
View File
@@ -106,10 +106,12 @@ void mf_daq_send_report(void)
uint8_t foo = 0;
int16_t climb = -gps.ned_vel.z;
int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
struct UtmCoor_f utm = stateGetPositionEnu_f();
int32_t east = utm.east * 100;
int32_t north = utm.north * 100;
DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix,
&gps.utm_pos.east, &gps.utm_pos.north,
&course, &gps.hmsl, &gps.gspeed, &climb,
&gps.week, &gps.tow, &gps.utm_pos.zone, &foo);
&east, &north, &course, &gps.hmsl, &gps.gspeed, &climb,
&gps.week, &gps.tow, &utm.zone, &foo);
}
}
sw/airborne/subsystems/gps.c
+30
View File
@@ -199,3 +199,33 @@ uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks)
void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t length __attribute__((unused)), uint8_t *data __attribute__((unused))){
}
/**
* Convenience function to get utm position from GPS state
*/
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
{
struct UtmCoor_f utm;
utm.alt = 0.f;
if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
// A real UTM position is available, use the correct zone
utm.zone = gps_s->utm_pos.zone;
utm.east = gps_s->utm_pos.east / 100.0f;
utm.north = gps_s->utm_pos.north / 100.0f;
}
else {
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps_s->lla_pos);
// Check if zone should be computed
if (zone > 0) {
utm.zone = zone;
} else {
utm.zone = (gps_s->lla_pos.lon / 1e7 + 180) / 6 + 1;
}
/* Recompute UTM coordinates in this zone */
utm_of_lla_f(&utm, &lla);
}
return utm;
}
sw/airborne/subsystems/gps.h
+10
View File
@@ -30,6 +30,7 @@
#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "math/pprz_geodetic_float.h"
#include "mcu_periph/sys_time.h"
@@ -162,4 +163,13 @@ extern struct GpsTimeSync gps_time_sync;
*/
extern uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks);
/**
* Convenience function to get utm position in float from GPS structure.
* Beware that altitude is initialized to zero but not set to the correct value
* @param[in] gps pointer to the gps structure
* @param[in] zone set the utm zone in which the position should be computed, 0 to try to get it automatically from lla position
* @return utm position in float
*/
extern struct UtmCoor_f utm_float_from_gps(struct GpsState *gps, uint8_t zone);
#endif /* GPS_H */
sw/airborne/subsystems/ins.c
+1 -14
View File
@@ -72,20 +72,7 @@ void ins_init(void)
void WEAK ins_reset_local_origin(void)
{
#if USE_GPS
struct UtmCoor_f utm;
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
}
else {
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
}
struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
sw/airborne/subsystems/ins/ins_alt_float.c
+3 -18
View File
@@ -113,20 +113,8 @@ void ins_alt_float_init(void)
/** Reset the geographic reference to the current GPS fix */
void ins_reset_local_origin(void)
{
struct UtmCoor_f utm;
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
}
else {
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
}
// get utm pos
struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
@@ -198,10 +186,7 @@ void ins_alt_float_update_baro(float pressure __attribute__((unused)))
void ins_alt_float_update_gps(struct GpsState *gps_s)
{
#if USE_GPS
struct UtmCoor_f utm;
utm.east = gps_s->utm_pos.east / 100.0f;
utm.north = gps_s->utm_pos.north / 100.0f;
utm.zone = nav_utm_zone0;
struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
#if !USE_BAROMETER
#ifdef GPS_DT
sw/airborne/subsystems/ins/ins_float_invariant.c
+4 -16
View File
@@ -273,20 +273,7 @@ void ins_float_invariant_init(void)
void ins_reset_local_origin(void)
{
#if INS_FINV_USE_UTM
struct UtmCoor_f utm;
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
}
else {
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
struct UtmCoor_f utm;
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
}
struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
// reset state UTM ref
@@ -441,9 +428,10 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s)
#if INS_FINV_USE_UTM
if (state.utm_initialized_f) {
struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
// position (local ned)
ins_float_inv.meas.pos_gps.x = (gps_s->utm_pos.north / 100.0f) - state.utm_origin_f.north;
ins_float_inv.meas.pos_gps.y = (gps_s->utm_pos.east / 100.0f) - state.utm_origin_f.east;
ins_float_inv.meas.pos_gps.x = utm.north - state.utm_origin_f.north;
ins_float_inv.meas.pos_gps.y = utm.east - state.utm_origin_f.east;
ins_float_inv.meas.pos_gps.z = state.utm_origin_f.alt - (gps_s->hmsl / 1000.0f);
// speed
ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f;
sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c
+2 -18
View File
@@ -46,20 +46,7 @@ void ins_gps_utm_init(void)
void ins_reset_local_origin(void)
{
struct UtmCoor_f utm;
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
}
else {
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
}
struct UtmCoor_f utm = utm_float_from_gps(&gps, 0);
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
@@ -82,10 +69,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct GpsState *gps_s)
{
struct UtmCoor_f utm;
utm.east = gps_s->utm_pos.east / 100.0f;
utm.north = gps_s->utm_pos.north / 100.0f;
utm.zone = nav_utm_zone0;
struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0);
utm.alt = gps_s->hmsl / 1000.0f;
// set position