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);
}
