diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index 51571751be..66713eeb4c 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -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; diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c index 9bf1bb1f38..f2cc999783 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.c +++ b/sw/airborne/subsystems/gps/gps_datalink.c @@ -32,10 +32,7 @@ #include "subsystems/gps.h" #include "subsystems/abi.h" -// #include - 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(<p_def, &ecef_nav0); + ltp_def_from_lla_i(<p_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 , <p_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, <p_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); } diff --git a/sw/airborne/subsystems/gps/gps_datalink.h b/sw/airborne/subsystems/gps/gps_datalink.h index f71a2ad239..93fa1dcf6a 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.h +++ b/sw/airborne/subsystems/gps/gps_datalink.h @@ -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 */ diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c index 6f6ce51607..a0e7dad780 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.c +++ b/sw/airborne/subsystems/gps/gps_mtk.c @@ -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);