2015-06-29 02:59:53 -03:00
|
|
|
#include "LR_MsgHandler.h"
|
2016-05-16 04:21:52 -03:00
|
|
|
#include "LogReader.h"
|
2016-05-25 07:46:18 -03:00
|
|
|
#include "Replay.h"
|
2015-06-14 04:17:42 -03:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
#include <AP_DAL/AP_DAL.h>
|
2020-03-09 00:28:23 -03:00
|
|
|
|
2019-06-11 23:39:18 -03:00
|
|
|
#include <cinttypes>
|
|
|
|
|
2015-06-14 04:17:42 -03:00
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
#define MSG_CREATE(sname,msgbytes) log_ ##sname msg; memcpy((void*)&msg, (msgbytes)+3, sizeof(msg));
|
2015-06-14 04:17:42 -03:00
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
LR_MsgHandler::LR_MsgHandler(struct log_Format &_f) :
|
|
|
|
MsgHandler(_f) {
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RFRH::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RFRH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RFRF::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RFRF, msgbytes);
|
2020-11-15 23:34:44 -04:00
|
|
|
#define MAP_FLAG(flag1, flag2) if (msg.frame_types & uint8_t(flag1)) msg.frame_types |= uint8_t(flag2)
|
|
|
|
/*
|
|
|
|
when we force an EKF we map the trigger flags over
|
|
|
|
*/
|
|
|
|
if (replay_force_ekf2) {
|
|
|
|
MAP_FLAG(AP_DAL::FrameType::InitialiseFilterEKF3, AP_DAL::FrameType::InitialiseFilterEKF2);
|
|
|
|
MAP_FLAG(AP_DAL::FrameType::UpdateFilterEKF3, AP_DAL::FrameType::UpdateFilterEKF2);
|
|
|
|
MAP_FLAG(AP_DAL::FrameType::LogWriteEKF3, AP_DAL::FrameType::LogWriteEKF2);
|
|
|
|
}
|
|
|
|
if (replay_force_ekf3) {
|
|
|
|
MAP_FLAG(AP_DAL::FrameType::InitialiseFilterEKF2, AP_DAL::FrameType::InitialiseFilterEKF3);
|
|
|
|
MAP_FLAG(AP_DAL::FrameType::UpdateFilterEKF2, AP_DAL::FrameType::UpdateFilterEKF3);
|
|
|
|
MAP_FLAG(AP_DAL::FrameType::LogWriteEKF2, AP_DAL::FrameType::LogWriteEKF3);
|
|
|
|
}
|
|
|
|
#undef MAP_FLAG
|
2020-11-08 22:06:04 -04:00
|
|
|
AP::dal().handle_message(msg, ekf2, ekf3);
|
2020-03-09 00:28:23 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RFRN::process_message(uint8_t *msgbytes)
|
2020-11-05 19:25:17 -04:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RFRN, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2020-11-05 19:25:17 -04:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_REV2::process_message(uint8_t *msgbytes)
|
2020-11-05 19:25:17 -04:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(REV2, msgbytes);
|
2020-11-05 19:25:17 -04:00
|
|
|
|
2020-11-15 23:34:44 -04:00
|
|
|
switch ((AP_DAL::Event)msg.event) {
|
2020-11-05 19:25:17 -04:00
|
|
|
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::resetGyroBias:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.resetGyroBias();
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::resetHeightDatum:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.resetHeightDatum();
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::setTakeoffExpected:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.setTakeoffExpected(true);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::unsetTakeoffExpected:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.setTakeoffExpected(false);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::setTouchdownExpected:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.setTouchdownExpected(true);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::unsetTouchdownExpected:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.setTouchdownExpected(false);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::setTerrainHgtStable:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.setTerrainHgtStable(true);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::unsetTerrainHgtStable:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.setTerrainHgtStable(false);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::requestYawReset:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.requestYawReset();
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::checkLaneSwitch:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.checkLaneSwitch();
|
|
|
|
break;
|
2016-05-04 21:36:50 -03:00
|
|
|
}
|
2020-11-15 23:34:44 -04:00
|
|
|
if (replay_force_ekf3) {
|
|
|
|
LR_MsgHandler_REV3 h{f, ekf2, ekf3};
|
|
|
|
h.process_message(msgbytes);
|
|
|
|
}
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RSO2::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RSO2, msgbytes);
|
2020-11-05 19:25:17 -04:00
|
|
|
Location loc;
|
2020-11-08 22:06:04 -04:00
|
|
|
loc.lat = msg.lat;
|
|
|
|
loc.lng = msg.lng;
|
|
|
|
loc.alt = msg.alt;
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf2.setOriginLLH(loc);
|
2020-11-15 23:34:44 -04:00
|
|
|
|
|
|
|
if (replay_force_ekf3) {
|
|
|
|
LR_MsgHandler_RSO2 h{f, ekf2, ekf3};
|
|
|
|
h.process_message(msgbytes);
|
|
|
|
}
|
2020-11-05 19:25:17 -04:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RWA2::process_message(uint8_t *msgbytes)
|
2020-11-05 19:25:17 -04:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RWA2, msgbytes);
|
|
|
|
ekf2.writeDefaultAirSpeed(msg.airspeed);
|
2020-11-15 23:34:44 -04:00
|
|
|
if (replay_force_ekf3) {
|
|
|
|
LR_MsgHandler_RWA2 h{f, ekf2, ekf3};
|
|
|
|
h.process_message(msgbytes);
|
|
|
|
}
|
2020-11-05 19:25:17 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_REV3::process_message(uint8_t *msgbytes)
|
2020-11-05 19:25:17 -04:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(REV3, msgbytes);
|
2020-11-05 19:25:17 -04:00
|
|
|
|
2020-11-15 23:34:44 -04:00
|
|
|
switch ((AP_DAL::Event)msg.event) {
|
2020-11-05 19:25:17 -04:00
|
|
|
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::resetGyroBias:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.resetGyroBias();
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::resetHeightDatum:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.resetHeightDatum();
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::setTakeoffExpected:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.setTakeoffExpected(true);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::unsetTakeoffExpected:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.setTakeoffExpected(false);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::setTouchdownExpected:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.setTouchdownExpected(true);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::unsetTouchdownExpected:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.setTouchdownExpected(false);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::setTerrainHgtStable:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.setTerrainHgtStable(true);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::unsetTerrainHgtStable:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.setTerrainHgtStable(false);
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::requestYawReset:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.requestYawReset();
|
|
|
|
break;
|
2020-11-15 23:34:44 -04:00
|
|
|
case AP_DAL::Event::checkLaneSwitch:
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.checkLaneSwitch();
|
|
|
|
break;
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
2020-11-15 23:34:44 -04:00
|
|
|
|
|
|
|
if (replay_force_ekf2) {
|
|
|
|
LR_MsgHandler_REV2 h{f, ekf2, ekf3};
|
|
|
|
h.process_message(msgbytes);
|
|
|
|
}
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RSO3::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RSO3, msgbytes);
|
2020-11-05 19:25:17 -04:00
|
|
|
Location loc;
|
2020-11-08 22:06:04 -04:00
|
|
|
loc.lat = msg.lat;
|
|
|
|
loc.lng = msg.lng;
|
|
|
|
loc.alt = msg.alt;
|
2020-11-05 19:25:17 -04:00
|
|
|
ekf3.setOriginLLH(loc);
|
2020-11-15 23:34:44 -04:00
|
|
|
if (replay_force_ekf2) {
|
|
|
|
LR_MsgHandler_RSO2 h{f, ekf2, ekf3};
|
|
|
|
h.process_message(msgbytes);
|
|
|
|
}
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RWA3::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RWA3, msgbytes);
|
|
|
|
ekf3.writeDefaultAirSpeed(msg.airspeed);
|
2020-11-15 23:34:44 -04:00
|
|
|
if (replay_force_ekf2) {
|
|
|
|
LR_MsgHandler_RWA2 h{f, ekf2, ekf3};
|
|
|
|
h.process_message(msgbytes);
|
|
|
|
}
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_REY3::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(REY3, msgbytes);
|
|
|
|
ekf3.writeEulerYawAngle(msg.yawangle, msg.yawangleerr, msg.timestamp_ms, msg.type);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RISH::process_message(uint8_t *msgbytes)
|
2016-05-04 05:16:25 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RISH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2016-05-04 05:16:25 -03:00
|
|
|
}
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RISI::process_message(uint8_t *msgbytes)
|
2016-05-04 05:16:25 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RISI, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2016-05-04 05:16:25 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RASH::process_message(uint8_t *msgbytes)
|
2016-05-04 05:16:25 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RASH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2016-05-04 05:16:25 -03:00
|
|
|
}
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RASI::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RASI, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RBRH::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RBRH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
2020-11-08 22:06:04 -04:00
|
|
|
|
|
|
|
void LR_MsgHandler_RBRI::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RBRI, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RRNH::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RRNH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
2020-11-08 22:06:04 -04:00
|
|
|
|
|
|
|
void LR_MsgHandler_RRNI::process_message(uint8_t *msgbytes)
|
2015-06-15 05:13:23 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RRNI, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-15 05:13:23 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RGPH::process_message(uint8_t *msgbytes)
|
2015-06-15 05:13:23 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RGPH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-15 05:13:23 -03:00
|
|
|
}
|
2020-11-08 22:06:04 -04:00
|
|
|
|
|
|
|
void LR_MsgHandler_RGPI::process_message(uint8_t *msgbytes)
|
2015-06-15 05:13:23 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RGPI, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-15 05:13:23 -03:00
|
|
|
}
|
2020-11-08 22:06:04 -04:00
|
|
|
|
|
|
|
void LR_MsgHandler_RGPJ::process_message(uint8_t *msgbytes)
|
2015-06-15 05:13:23 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RGPJ, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-15 05:13:23 -03:00
|
|
|
}
|
2015-06-14 04:17:42 -03:00
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RMGH::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RMGH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
2020-11-08 22:06:04 -04:00
|
|
|
|
|
|
|
void LR_MsgHandler_RMGI::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RMGI, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RBCH::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RBCH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
2020-11-08 22:06:04 -04:00
|
|
|
|
|
|
|
void LR_MsgHandler_RBCI::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RBCI, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RVOH::process_message(uint8_t *msgbytes)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RVOH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_ROFH::process_message(uint8_t *msgbytes)
|
2020-11-06 18:29:30 -04:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(ROFH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg, ekf2, ekf3);
|
2020-11-06 18:29:30 -04:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RWOH::process_message(uint8_t *msgbytes)
|
2020-11-06 20:33:40 -04:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RWOH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg, ekf2, ekf3);
|
2020-11-06 20:33:40 -04:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_RBOH::process_message(uint8_t *msgbytes)
|
2020-11-07 01:57:11 -04:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(RBOH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg, ekf2, ekf3);
|
2020-11-07 01:57:11 -04:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_REPH::process_message(uint8_t *msgbytes)
|
2020-11-06 19:57:07 -04:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(REPH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg, ekf2, ekf3);
|
2020-11-06 19:57:07 -04:00
|
|
|
}
|
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
void LR_MsgHandler_REVH::process_message(uint8_t *msgbytes)
|
2020-11-06 19:57:07 -04:00
|
|
|
{
|
2020-11-08 22:06:04 -04:00
|
|
|
MSG_CREATE(REVH, msgbytes);
|
|
|
|
AP::dal().handle_message(msg, ekf2, ekf3);
|
2020-11-06 19:57:07 -04:00
|
|
|
}
|
|
|
|
|
2020-11-05 19:25:17 -04:00
|
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
|
|
#include "VehicleType.h"
|
2015-06-14 04:17:42 -03:00
|
|
|
|
2015-10-26 03:37:16 -03:00
|
|
|
bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
|
2015-06-14 04:17:42 -03:00
|
|
|
{
|
2020-11-05 19:25:17 -04:00
|
|
|
const char *ignore_parms[] = {
|
|
|
|
"LOG_FILE_BUFSIZE",
|
|
|
|
"LOG_DISARMED"
|
|
|
|
};
|
2015-07-06 12:30:59 -03:00
|
|
|
for (uint8_t i=0; i < ARRAY_SIZE(ignore_parms); i++) {
|
2015-06-14 04:17:42 -03:00
|
|
|
if (strncmp(name, ignore_parms[i], AP_MAX_NAME_SIZE) == 0) {
|
|
|
|
::printf("Ignoring set of %s to %f\n", name, value);
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
}
|
2015-10-26 03:37:16 -03:00
|
|
|
|
2020-11-08 22:06:04 -04:00
|
|
|
return LogReader::set_parameter(name, value);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
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];
|
|
|
|
|
|
|
|
require_field(msg, "Name", parameter_name, parameter_name_len);
|
|
|
|
|
2016-05-16 04:21:52 -03:00
|
|
|
float value = require_field_float(msg, "Value");
|
2020-11-05 19:25:17 -04:00
|
|
|
set_parameter(parameter_name, value);
|
2015-06-14 04:17:42 -03:00
|
|
|
}
|