Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 13:24:03 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[jgps] default update for gps tow when changed and updated utm (#1637)

Browse Source
This commit is contained in:
Kirk Scheper
2016-04-25 14:25:29 +02:00
committed by Felix Ruess
parent 80067e6b61
commit 063d361055
4 changed files with 43 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/gps.c
+14 -26
View File
@@ -231,6 +231,11 @@ static void gps_cb(uint8_t sender_id,
gps = *gps_s;
AbiSendMsgGPS(GPS_MULTI_ID, now_ts, gps_s);
#endif
if (gps.tow != gps_time_sync.t0_tow)
{
gps_time_sync.t0_ticks = sys_time.nb_tick;
gps_time_sync.t0_tow = gps.tow;
}
}
/*
@@ -342,25 +347,20 @@ void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t len
struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
{
struct UtmCoor_f utm;
utm.alt = 0.f;
utm.zone = zone;
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;
utm.alt = gps_s->utm_pos.alt / 1000.f;
}
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);
struct UtmCoor_i utm_i;
utm_of_lla_i(&utm_i, &gps_s->lla_pos);
UTM_FLOAT_OF_BFP(utm, utm_i);
}
return utm;
@@ -369,30 +369,18 @@ struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone)
{
struct UtmCoor_i utm;
utm.alt = 0;
utm.zone = zone;
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;
utm.alt = gps_s->utm_pos.alt;
}
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;
utm_of_lla_i(&utm, &gps_s->lla_pos);
}
return utm;
sw/airborne/subsystems/gps/gps_datalink.c
+28 -31
View File
@@ -32,10 +32,7 @@
#include "subsystems/gps.h"
#include "subsystems/abi.h"
// #include <stdio.h>
struct LtpDef_i ltp_def;
struct EnuCoor_i enu_pos, enu_speed;
struct GpsState gps_datalink;
@@ -43,8 +40,10 @@ struct GpsState gps_datalink;
void gps_datalink_init(void)
{
gps_datalink.fix = GPS_FIX_NONE;
gps_datalink.gspeed = 700; // To enable course setting
gps_datalink.cacc = 0; // To enable course setting
gps_datalink.pdop = 0;
gps_datalink.sacc = 0;
gps_datalink.pacc = 0;
gps_datalink.cacc = 0;
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = NAV_LAT0;
@@ -52,14 +51,14 @@ void gps_datalink_init(void)
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
ltp_def_from_ecef_i(&ltp_def, &ecef_nav0);
ltp_def_from_lla_i(&ltp_def, &llh_nav0);
}
// Parse the REMOTE_GPS_SMALL datalink packet
void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading)
void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t speed_xyz, uint32_t tow)
{
struct EnuCoor_i enu_pos, enu_speed;
// Position in ENU coordinates
enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm
if (enu_pos.x & 0x400) {
@@ -91,13 +90,14 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements
}
VECT3_NED_OF_ENU(gps_datalink.ned_vel, enu_speed);
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT);
ecef_of_enu_vect_i(&gps_datalink.ecef_vel , &ltp_def , &enu_speed);
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps_datalink.ned_vel.x = enu_speed.y;
gps_datalink.ned_vel.y = enu_speed.x;
gps_datalink.ned_vel.z = -enu_speed.z;
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT);
gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(enu_speed);
gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(enu_speed);
gps_datalink.hmsl = ltp_def.hmsl + enu_pos.z * 10;
SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT);
@@ -105,18 +105,18 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
gps_datalink.course = ((int32_t)heading) * 1e3;
SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
gps_datalink.num_sv = num_sv;
gps_datalink.tow = gps_tow_from_sys_ticks(sys_time.nb_tick);
gps_datalink.num_sv = 7;
gps_datalink.tow = tow;
gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps_datalink.last_msg_ticks = sys_time.nb_sec_rem;
gps_datalink.last_msg_time = sys_time.nb_sec;
if (gps_datalink.fix == GPS_FIX_3D) {
gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_datalink.last_3dfix_time = sys_time.nb_sec;
}
gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_datalink.last_3dfix_time = sys_time.nb_sec;
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
}
@@ -143,30 +143,27 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
gps_datalink.ecef_vel.z = ecef_zd;
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps_datalink.ned_vel.x = enu_speed.y;
gps_datalink.ned_vel.y = enu_speed.x;
gps_datalink.ned_vel.z = -enu_speed.z;
ned_of_ecef_vect_i(&gps_datalink.ned_vel, &ltp_def , &gps_datalink.ecef_vel);
SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT);
gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(gps_datalink.ned_vel);
gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(gps_datalink.ned_vel);
gps_datalink.course = course;
SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
gps_datalink.num_sv = numsv;
if (tow == 0) {
gps_datalink.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
} else {
gps_datalink.tow = gps_tow_from_sys_ticks(sys_time.nb_tick); //tow;
}
gps_datalink.tow = tow;
gps_datalink.fix = GPS_FIX_3D;
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps_datalink.last_msg_ticks = sys_time.nb_sec_rem;
gps_datalink.last_msg_time = sys_time.nb_sec;
if (gps_datalink.fix == GPS_FIX_3D) {
gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_datalink.last_3dfix_time = sys_time.nb_sec;
}
gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_datalink.last_3dfix_time = sys_time.nb_sec;
AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
}
sw/airborne/subsystems/gps/gps_datalink.h
+1 -2
View File
@@ -43,12 +43,11 @@ extern struct GpsState gps_datalink;
extern void gps_datalink_init(void);
extern void gps_datalink_register(void);
extern void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_xyz, int16_t heading);
extern void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t speed_xyz, uint32_t tow);
extern void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z,
int32_t lat, int32_t lon, int32_t alt, int32_t hmsl,
int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd,
uint32_t tow, int32_t course);
#endif /* GPS_DATALINK_H */
sw/airborne/subsystems/gps/gps_mtk.c
-3
View File
@@ -209,9 +209,6 @@ void gps_mtk_read_message(void)
if (gps_mtk.msg_class == MTK_DIY14_ID) {
if (gps_mtk.msg_id == MTK_DIY14_NAV_ID) {
/* get hardware clock ticks */
gps_time_sync.t0_ticks = sys_time.nb_tick;
gps_time_sync.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
gps_time_sync.t0_tow_frac = 0;
gps_mtk.state.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf) * 10;
gps_mtk.state.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf) * 10;
SetBit(gps_mtk.state.valid_fields, GPS_VALID_POS_LLA_BIT);