diff --git a/conf/modules/traffic_info.xml b/conf/modules/traffic_info.xml index a639323218..d960586a86 100644 --- a/conf/modules/traffic_info.xml +++ b/conf/modules/traffic_info.xml @@ -9,11 +9,12 @@ - - - - - + + + + + + diff --git a/sw/airborne/modules/multi/traffic_info.c b/sw/airborne/modules/multi/traffic_info.c index e8dc89d738..aa99464990 100644 --- a/sw/airborne/modules/multi/traffic_info.c +++ b/sw/airborne/modules/multi/traffic_info.c @@ -136,6 +136,36 @@ bool parse_acinfo_dl(uint8_t *buf) DL_GPS_LLA_itow(buf)); } break; + case DL_GPS_INT: { + struct LtpDef_i def = {0}; + struct EcefCoor_i ecef_pos = { + .x = DL_GPS_INT_ecef_x(buf), + .y = DL_GPS_INT_ecef_y(buf), + .z = DL_GPS_INT_ecef_z(buf), + }; + struct EcefCoor_i ecef_vel = { + .x = DL_GPS_INT_ecef_xd(buf), + .y = DL_GPS_INT_ecef_yd(buf), + .z = DL_GPS_INT_ecef_zd(buf), + }; + struct NedCoor_i ned_vel; + ltp_def_from_ecef_i(&def, &ecef_pos); + ned_of_ecef_vect_i(&ned_vel, &def, &ecef_vel); + struct NedCoor_f ned_vel_f; + VECT3_FLOAT_OF_CM(ned_vel_f, ned_vel); + int16_t course = (int16_t)(10.f * DegOfRad(atan2f(ned_vel_f.y, ned_vel_f.x))); // decideg + uint16_t gspeed = (uint16_t)(CM_OF_M(FLOAT_VECT2_NORM(ned_vel_f))); + int16_t climb = (int16_t)(CM_OF_M(-ned_vel_f.z)); + set_ac_info_lla(sender_id, + DL_GPS_INT_lat(buf), + DL_GPS_INT_lon(buf), + DL_GPS_INT_alt(buf), + course, + gspeed, + climb, + DL_GPS_INT_tow(buf)); + } + break; default: return FALSE; } @@ -182,6 +212,10 @@ void set_ac_info_utm(uint8_t id, uint32_t utm_east, uint32_t utm_north, uint32_t ti_acs[ti_acs_id[id]].ac_id = id; } + if (itow < ti_acs[ti_acs_id[id]].itow) { + return; // don't update on old data + } + ti_acs[ti_acs_id[id]].status = 0; uint16_t my_zone = stateGetUtmOrigin_f()->zone; @@ -224,6 +258,10 @@ void set_ac_info_lla(uint8_t id, int32_t lat, int32_t lon, int32_t alt, ti_acs[ti_acs_id[id]].ac_id = id; } + if (itow < ti_acs[ti_acs_id[id]].itow) { + return; // don't update on old data + } + ti_acs[ti_acs_id[id]].status = 0; struct LlaCoor_i lla = {.lat = lat, .lon = lon, .alt = alt};