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

[gps] fix sending of UTM values in GPS message

Browse Source 
- add convenience function to get utm in int from GPS struct

fix stuff missed in #1476
This commit is contained in:
Felix Ruess
2015-12-28 20:17:19 +01:00
parent 3f4d1e1a59
commit 3c2e017e24
2 changed files with 45 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/gps.c
+36 -3
View File
@@ -25,7 +25,7 @@
*/
#include "subsystems/gps.h"
#include "subsystems/navigation/common_nav.h"
#include "led.h"
#ifdef GPS_POWER_GPIO
@@ -90,10 +90,11 @@ static void send_gps(struct transport_tx *trans, struct link_device *dev)
uint8_t zero = 0;
int16_t climb = -gps.ned_vel.z;
int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6));
struct UtmCoor_i utm = utm_int_from_gps(&gps, nav_utm_zone0);
pprz_msg_send_GPS(trans, dev, AC_ID, &gps.fix,
&gps.utm_pos.east, &gps.utm_pos.north,
&utm.east, &utm.north,
&course, &gps.hmsl, &gps.gspeed, &climb,
&gps.week, &gps.tow, &gps.utm_pos.zone, &zero);
&gps.week, &gps.tow, &utm.zone, &zero);
// send SVINFO for available satellites that have new data
send_svinfo_available(trans, dev);
}
@@ -229,3 +230,35 @@ struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
return utm;
}
struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone)
{
struct UtmCoor_i utm;
utm.alt = 0;
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;
utm.north = gps_s->utm_pos.north;
}
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 */
struct UtmCoor_f utm_f;
utm_f.zone = utm.zone;
utm_of_lla_f(&utm_f, &lla);
/* convert to fixed point in cm */
utm.east = utm_f.east * 100;
utm.north = utm_f.north * 100;
}
return utm;
}
sw/airborne/subsystems/gps.h
+9
View File
@@ -172,4 +172,13 @@ extern uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks);
*/
extern struct UtmCoor_f utm_float_from_gps(struct GpsState *gps, uint8_t zone);
/**
* Convenience function to get utm position in int 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 fixed point (cm)
*/
extern struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone);
#endif /* GPS_H */