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

[gps_ubx] make state part of gps_ubx

Browse Source
This commit is contained in:
Felix Ruess
2016-01-28 22:56:33 +01:00
committed by masierra
parent 95f2b47b15
commit 6db2cc9f43
2 changed files with 61 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/gps/gps_ubx.c
+60 -61
View File
@@ -56,7 +56,6 @@ struct GpsUbx gps_ubx;
struct GpsUbxRaw gps_ubx_raw;
#endif
struct GpsState gps_ubx_state;
struct GpsTimeSync gps_ubx_time_sync;
void ubx_gps_impl_init(void)
@@ -66,7 +65,7 @@ void ubx_gps_impl_init(void)
gps_ubx.error_cnt = 0;
gps_ubx.error_last = GPS_UBX_ERR_NONE;
gps_ubx_state.comp_id = GPS_UBX_ID;
gps_ubx.state.comp_id = GPS_UBX_ID;
}
void ubx_gps_event(void)
@@ -90,80 +89,80 @@ void gps_ubx_read_message(void)
gps_ubx_time_sync.t0_ticks = sys_time.nb_tick;
gps_ubx_time_sync.t0_tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
gps_ubx_time_sync.t0_tow_frac = UBX_NAV_SOL_Frac(gps_ubx.msg_buf);
gps_ubx_state.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
gps_ubx_state.week = UBX_NAV_SOL_week(gps_ubx.msg_buf);
gps_ubx_state.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf);
gps_ubx_state.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf);
gps_ubx_state.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf);
gps_ubx_state.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf);
SetBit(gps_ubx_state.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps_ubx_state.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf);
gps_ubx_state.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf);
gps_ubx_state.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf);
gps_ubx_state.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf);
SetBit(gps_ubx_state.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps_ubx_state.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf);
gps_ubx_state.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf);
gps_ubx_state.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf);
gps_ubx.state.tow = UBX_NAV_SOL_ITOW(gps_ubx.msg_buf);
gps_ubx.state.week = UBX_NAV_SOL_week(gps_ubx.msg_buf);
gps_ubx.state.fix = UBX_NAV_SOL_GPSfix(gps_ubx.msg_buf);
gps_ubx.state.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf);
gps_ubx.state.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf);
gps_ubx.state.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf);
SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps_ubx.state.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf);
gps_ubx.state.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf);
gps_ubx.state.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf);
gps_ubx.state.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf);
SetBit(gps_ubx.state.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps_ubx.state.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf);
gps_ubx.state.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf);
gps_ubx.state.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf);
#ifdef GPS_LED
if (gps_ubx_state.fix == GPS_FIX_3D) {
if (gps_ubx.state.fix == GPS_FIX_3D) {
LED_ON(GPS_LED);
} else {
LED_TOGGLE(GPS_LED);
}
#endif
} else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) {
gps_ubx_state.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf);
gps_ubx_state.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf);
gps_ubx_state.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf);
SetBit(gps_ubx_state.valid_fields, GPS_VALID_POS_LLA_BIT);
gps_ubx_state.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf);
SetBit(gps_ubx_state.valid_fields, GPS_VALID_HMSL_BIT);
gps_ubx.state.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf);
gps_ubx.state.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf);
gps_ubx.state.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf);
SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_LLA_BIT);
gps_ubx.state.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf);
SetBit(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT);
} else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) {
gps_ubx_state.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf);
gps_ubx_state.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf);
gps_ubx.state.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf);
gps_ubx.state.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf);
uint8_t hem = UBX_NAV_POSUTM_HEM(gps_ubx.msg_buf);
if (hem == UTM_HEM_SOUTH) {
gps_ubx_state.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
gps_ubx.state.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
}
gps_ubx_state.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf) * 10;
gps_ubx_state.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf);
SetBit(gps_ubx_state.valid_fields, GPS_VALID_POS_UTM_BIT);
gps_ubx.state.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf) * 10;
gps_ubx.state.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf);
SetBit(gps_ubx.state.valid_fields, GPS_VALID_POS_UTM_BIT);
gps_ubx_state.hmsl = gps_ubx_state.utm_pos.alt;
SetBit(gps_ubx_state.valid_fields, GPS_VALID_HMSL_BIT);
gps_ubx.state.hmsl = gps_ubx.state.utm_pos.alt;
SetBit(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT);
/* with UTM only you do not receive ellipsoid altitude, so set only if no valid lla */
if (!bit_is_set(gps_ubx_state.valid_fields, GPS_VALID_HMSL_BIT)) {
gps_ubx_state.lla_pos.alt = gps_ubx_state.utm_pos.alt;
if (!bit_is_set(gps_ubx.state.valid_fields, GPS_VALID_HMSL_BIT)) {
gps_ubx.state.lla_pos.alt = gps_ubx.state.utm_pos.alt;
}
} else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) {
gps_ubx_state.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf);
gps_ubx_state.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf);
gps_ubx_state.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf);
gps_ubx_state.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf);
gps_ubx_state.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf);
SetBit(gps_ubx_state.valid_fields, GPS_VALID_VEL_NED_BIT);
gps_ubx.state.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf);
gps_ubx.state.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf);
gps_ubx.state.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf);
gps_ubx.state.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf);
gps_ubx.state.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf);
SetBit(gps_ubx.state.valid_fields, GPS_VALID_VEL_NED_BIT);
// Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
// I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
// solution: First to radians, and then scale to 1e-7 radians
// First x 10 for loosing less resolution, then to radians, then multiply x 10 again
gps_ubx_state.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) * 10)) * 10;
SetBit(gps_ubx_state.valid_fields, GPS_VALID_COURSE_BIT);
gps_ubx_state.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf) * 10)) * 10;
gps_ubx_state.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf);
gps_ubx.state.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) * 10)) * 10;
SetBit(gps_ubx.state.valid_fields, GPS_VALID_COURSE_BIT);
gps_ubx.state.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf) * 10)) * 10;
gps_ubx.state.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf);
} else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) {
gps_ubx_state.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS);
gps_ubx.state.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS);
uint8_t i;
for (i = 0; i < gps_ubx_state.nb_channels; i++) {
gps_ubx_state.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i);
gps_ubx_state.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i);
gps_ubx_state.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i);
gps_ubx_state.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i);
gps_ubx_state.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i);
gps_ubx_state.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i);
for (i = 0; i < gps_ubx.state.nb_channels; i++) {
gps_ubx.state.svinfos[i].svid = UBX_NAV_SVINFO_SVID(gps_ubx.msg_buf, i);
gps_ubx.state.svinfos[i].flags = UBX_NAV_SVINFO_Flags(gps_ubx.msg_buf, i);
gps_ubx.state.svinfos[i].qi = UBX_NAV_SVINFO_QI(gps_ubx.msg_buf, i);
gps_ubx.state.svinfos[i].cno = UBX_NAV_SVINFO_CNO(gps_ubx.msg_buf, i);
gps_ubx.state.svinfos[i].elev = UBX_NAV_SVINFO_Elev(gps_ubx.msg_buf, i);
gps_ubx.state.svinfos[i].azim = UBX_NAV_SVINFO_Azim(gps_ubx.msg_buf, i);
}
} else if (gps_ubx.msg_id == UBX_NAV_STATUS_ID) {
gps_ubx_state.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf);
gps_ubx.state.fix = UBX_NAV_STATUS_GPSfix(gps_ubx.msg_buf);
gps_ubx.status_flags = UBX_NAV_STATUS_Flags(gps_ubx.msg_buf);
gps_ubx.sol_flags = UBX_NAV_SOL_Flags(gps_ubx.msg_buf);
}
@@ -332,17 +331,17 @@ void gps_ubx_msg(void)
// current timestamp
// uint32_t now_ts = get_sys_time_usec();
gps_ubx_state.last_msg_ticks = sys_time.nb_sec_rem;
gps_ubx_state.last_msg_time = sys_time.nb_sec;
gps_ubx.state.last_msg_ticks = sys_time.nb_sec_rem;
gps_ubx.state.last_msg_time = sys_time.nb_sec;
gps_ubx_read_message();
gps_ubx_ucenter_event();
if (gps_ubx.msg_class == UBX_NAV_ID &&
(gps_ubx.msg_id == UBX_NAV_VELNED_ID ||
(gps_ubx.msg_id == UBX_NAV_SOL_ID &&
!bit_is_set(gps_ubx_state.valid_fields, GPS_VALID_VEL_NED_BIT)))) {
if (gps_ubx_state.fix == GPS_FIX_3D) {
gps_ubx_state.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_ubx_state.last_3dfix_time = sys_time.nb_sec;
!bit_is_set(gps_ubx.state.valid_fields, GPS_VALID_VEL_NED_BIT)))) {
if (gps_ubx.state.fix == GPS_FIX_3D) {
gps_ubx.state.last_3dfix_ticks = sys_time.nb_sec_rem;
gps_ubx.state.last_3dfix_time = sys_time.nb_sec;
}
// AbiSendMsgGPS(GPS_UBX_ID, now_ts, &gps);
}
@@ -352,8 +351,8 @@ void gps_ubx_msg(void)
void ubx_gps_register(void)
{
#ifdef GPS_SECONDARY_UBX
gps_register_impl(ubx_gps_impl_init, ubx_gps_event, &gps_ubx_state, &gps_ubx_time_sync, 1);
gps_register_impl(ubx_gps_impl_init, ubx_gps_event, &gps_ubx.state, &gps_ubx_time_sync, 1);
#else
gps_register_impl(ubx_gps_impl_init, ubx_gps_event, &gps_ubx_state, &gps_ubx_time_sync, 0);
gps_register_impl(ubx_gps_impl_init, ubx_gps_event, &gps_ubx.state, &gps_ubx_time_sync, 0);
#endif
}
sw/airborne/subsystems/gps/gps_ubx.h
+1
View File
@@ -73,6 +73,7 @@ struct GpsUbx {
uint8_t status_flags;
uint8_t sol_flags;
struct GpsState state;
};
extern struct GpsUbx gps_ubx;