void LR_MsgHandler_ARSP::process_message(uint8_t *msg) { wait_timestamp_from_msg(msg); airspeed.setHIL(require_field_float(msg, "Airspeed"), require_field_float(msg, "DiffPress"), require_field_float(msg, "Temp")); }
void LR_MsgHandler_BARO::process_message(uint8_t *msg) { wait_timestamp_from_msg(msg); uint32_t last_update_ms; if (!field_value(msg, "SMS", last_update_ms)) { last_update_ms = 0; } baro.setHIL(0, require_field_float(msg, "Press"), require_field_int16_t(msg, "Temp") * 0.01f, require_field_float(msg, "Alt"), require_field_float(msg, "CRt"), last_update_ms); }
void LR_MsgHandler_BARO::process_message(uint8_t *msg) { wait_timestamp_from_msg(msg); baro.setHIL(0, require_field_float(msg, "Press"), require_field_int16_t(msg, "Temp") * 0.01f); }
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 MsgHandler::ground_vel_from_msg(uint8_t *msg, Vector3f &vel, const char *label_speed, const char *label_course, const char *label_vz) { uint32_t ground_speed; int32_t ground_course; require_field(msg, label_speed, ground_speed); require_field(msg, label_course, ground_course); vel[0] = ground_speed*0.01f*cosf(radians(ground_course*0.01f)); vel[1] = ground_speed*0.01f*sinf(radians(ground_course*0.01f)); vel[2] = require_field_float(msg, label_vz); }
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_NTUN_Copter::process_message(uint8_t *msg) { inavpos = Vector3f(require_field_float(msg, "PosX") * 0.01f, require_field_float(msg, "PosY") * 0.01f, 0); }