void LR_MsgHandler_PARM::process_message(uint8_t *msg) { const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term char parameter_name[parameter_name_len]; uint64_t time_us; if (field_value(msg, "TimeUS", time_us)) { wait_timestamp_usec(time_us); } else { // older logs can have a lot of FMT and PARM messages up the // front which don't have timestamps. Since in Replay we run // DataFlash's IO only when stop_clock is called, we can // overflow DataFlash's ringbuffer. This should force us to // do IO: hal.scheduler->stop_clock(last_timestamp_usec); } require_field(msg, "Name", parameter_name, parameter_name_len); float value = require_field_float(msg, "Value"); if (globals.no_params) { printf("Not changing %s to %f\n", parameter_name, value); } else { set_parameter(parameter_name, value); } }
void LR_MsgHandler::wait_timestamp_from_msg(uint8_t *msg) { uint64_t time_us; uint32_t time_ms; if (field_value(msg, "TimeUS", time_us)) { // 64-bit timestamp present - great! wait_timestamp_usec(time_us); } else if (field_value(msg, "TimeMS", time_ms)) { // there is special rounding code that needs to be crossed in // wait_timestamp: wait_timestamp(time_ms); } else { ::printf("No timestamp on message"); } }
void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt) { uint64_t time_us; if (! field_value(msg, "TimeUS", time_us)) { uint32_t timestamp; require_field(msg, "T", timestamp); time_us = timestamp * 1000; } wait_timestamp_usec(time_us); Location loc; location_from_msg(msg, loc, "Lat", "Lng", "Alt"); Vector3f vel; ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ"); uint8_t status = require_field_uint8_t(msg, "Status"); uint8_t hdop = 0; if (! field_value(msg, "HDop", hdop) && ! field_value(msg, "HDp", hdop)) { hdop = 20; } uint8_t nsats = 0; if (! field_value(msg, "NSats", nsats) && ! field_value(msg, "numSV", nsats)) { field_not_found(msg, "NSats"); } gps.setHIL(gps_offset, (AP_GPS::GPS_Status)status, uint32_t(time_us/1000), loc, vel, nsats, hdop, require_field_float(msg, "VZ") != 0); if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) { ground_alt_cm = require_field_int32_t(msg, "Alt"); } if (responsible_for_relalt) { // this could possibly check for the presence of "RelAlt" label? int32_t tmp; if (! field_value(msg, "RAlt", tmp)) { tmp = require_field_int32_t(msg, "RelAlt"); } rel_altitude = 0.01f * tmp; } }
void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg) { uint64_t time_us; if (! field_value(msg, "TimeUS", time_us)) { uint32_t timestamp; require_field(msg, "T", timestamp); time_us = timestamp * 1000; } wait_timestamp_usec(time_us); Location loc; location_from_msg(msg, loc, "Lat", "Lng", "Alt"); Vector3f vel; ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ"); uint8_t status = require_field_uint8_t(msg, "Status"); uint8_t hdop = 0; if (! field_value(msg, "HDop", hdop) && ! field_value(msg, "HDp", hdop)) { hdop = 20; } uint8_t nsats = 0; if (! field_value(msg, "NSats", nsats) && ! field_value(msg, "numSV", nsats)) { field_not_found(msg, "NSats"); } uint16_t GWk; uint32_t GMS; if (! field_value(msg, "GWk", GWk)) { field_not_found(msg, "GWk"); } if (! field_value(msg, "GMS", GMS)) { field_not_found(msg, "GMS"); } gps.setHIL(gps_offset, (AP_GPS::GPS_Status)status, AP_GPS::time_epoch_convert(GWk, GMS), loc, vel, nsats, hdop); if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) { ground_alt_cm = require_field_int32_t(msg, "Alt"); } }
void LR_MsgHandler_GPA_Base::update_from_msg_gpa(uint8_t gps_offset, uint8_t *msg) { uint64_t time_us; require_field(msg, "TimeUS", time_us); wait_timestamp_usec(time_us); uint16_t vdop, hacc, vacc, sacc; require_field(msg, "VDop", vdop); require_field(msg, "HAcc", hacc); require_field(msg, "VAcc", vacc); require_field(msg, "SAcc", sacc); uint8_t have_vertical_velocity; if (! field_value(msg, "VV", have_vertical_velocity)) { have_vertical_velocity = !is_zero(gps.velocity(gps_offset).z); } uint32_t sample_ms; if (! field_value(msg, "SMS", sample_ms)) { sample_ms = 0; } gps.setHIL_Accuracy(gps_offset, vdop*0.01f, hacc*0.01f, vacc*0.01f, sacc*0.01f, have_vertical_velocity, sample_ms); }
void LR_MsgHandler::wait_timestamp(uint32_t timestamp) { uint64_t usecs = timestamp*1000UL; wait_timestamp_usec(usecs); }