void LR_MsgHandler_IMT_Base::update_from_msg_imt(uint8_t imu_offset, uint8_t *msg) { wait_timestamp_from_msg(msg); if (!use_imt) { return; } uint8_t this_imu_mask = 1 << imu_offset; float delta_time = 0; require_field(msg, "DelT", delta_time); ins.set_delta_time(delta_time); if (gyro_mask & this_imu_mask) { Vector3f d_angle; require_field(msg, "DelA", d_angle); float d_angle_dt; if (!field_value(msg, "DelaT", d_angle_dt)) { d_angle_dt = 0; } ins.set_delta_angle(imu_offset, d_angle, d_angle_dt); } if (accel_mask & this_imu_mask) { float dvt = 0; require_field(msg, "DelvT", dvt); Vector3f d_velocity; require_field(msg, "DelV", d_velocity); ins.set_delta_velocity(imu_offset, dvt, d_velocity); } }
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_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_ARM::process_message(uint8_t *msg) { wait_timestamp_from_msg(msg); uint8_t ArmState = require_field_uint8_t(msg, "ArmState"); hal.util->set_soft_armed(ArmState); printf("Armed state: %u at %lu\n", (unsigned)ArmState, (unsigned long)AP_HAL::millis()); }
void LR_MsgHandler_CHEK::process_message(uint8_t *msg) { wait_timestamp_from_msg(msg); check_state.time_us = AP_HAL::micros64(); attitude_from_msg(msg, check_state.euler, "Roll", "Pitch", "Yaw"); check_state.euler *= radians(1); location_from_msg(msg, check_state.pos, "Lat", "Lng", "Alt"); require_field(msg, "VN", check_state.velocity.x); require_field(msg, "VE", check_state.velocity.y); require_field(msg, "VD", check_state.velocity.z); }
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_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg) { wait_timestamp_from_msg(msg); Vector3f mag; require_field(msg, "Mag", mag); Vector3f mag_offset; require_field(msg, "Ofs", mag_offset); compass.setHIL(compass_offset, mag - mag_offset); // compass_offset is which compass we are setting info for; // mag_offset is a vector indicating the compass' calibration... compass.set_offsets(compass_offset, mag_offset); }
void LR_MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg) { wait_timestamp_from_msg(msg); uint8_t this_imu_mask = 1 << imu_offset; if (gyro_mask & this_imu_mask) { Vector3f gyro; require_field(msg, "Gyr", gyro); ins.set_gyro(imu_offset, gyro); } if (accel_mask & this_imu_mask) { Vector3f accel2; require_field(msg, "Acc", accel2); ins.set_accel(imu_offset, accel2); } }
void LR_MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg) { wait_timestamp_from_msg(msg); Vector3f mag; require_field(msg, "Mag", mag); Vector3f mag_offset; require_field(msg, "Ofs", mag_offset); uint32_t last_update_usec; if (!field_value(msg, "S", last_update_usec)) { last_update_usec = AP_HAL::micros(); } compass.setHIL(compass_offset, mag - mag_offset, last_update_usec); // compass_offset is which compass we are setting info for; // mag_offset is a vector indicating the compass' calibration... compass.set_offsets(compass_offset, mag_offset); }
void LR_MsgHandler_ATT::process_message(uint8_t *msg) { wait_timestamp_from_msg(msg); attitude_from_msg(msg, attitude, "Roll", "Pitch", "Yaw"); }
void LR_MsgHandler_NKF1::process_message(uint8_t *msg) { wait_timestamp_from_msg(msg); }
void MsgHandler_SIM::process_message(uint8_t *msg) { wait_timestamp_from_msg(msg); attitude_from_msg(msg, sim_attitude, "Roll", "Pitch", "Yaw"); }