aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Replay: added support for EV message for copter arming

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-15 12:59:09 +10:00
parent 4e3d18bee4
commit 9f05e54d90
4 changed files with 38 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Tools/Replay/LogReader.cpp
View File

@ -27,6 +27,7 @@
#include "MsgHandler_SIM.h"
#include "MsgHandler_BARO.h"
#include "MsgHandler_ARM.h"
#include "MsgHandler_Event.h"
#include "MsgHandler_AHR2.h"
#include "MsgHandler_ATT.h"
#include "MsgHandler_MAG.h"
@ -165,6 +166,9 @@ bool LogReader::update(char type[5])
} else if (streq(name, "ARM")) {
msgparser[f.type] = new MsgHandler_ARM(formats[f.type], dataflash,
last_timestamp_usec);
} else if (streq(name, "EV")) {
msgparser[f.type] = new MsgHandler_Event(formats[f.type], dataflash,
last_timestamp_usec);
} else if (streq(name, "AHR2")) {
msgparser[f.type] = new MsgHandler_AHR2(formats[f.type], dataflash,
last_timestamp_usec,

21
Tools/Replay/MsgHandler_Event.cpp Normal file
View File

@ -0,0 +1,21 @@
#include "MsgHandler_Event.h"
extern const AP_HAL::HAL& hal;
#define DATA_ARMED 10
#define DATA_DISARMED 11
void MsgHandler_Event::process_message(uint8_t *msg)
{
uint8_t id = require_field_uint8_t(msg, "Id");
if (id == DATA_ARMED) {
hal.util->set_soft_armed(true);
printf("Armed at %lu\n",
(unsigned long)hal.scheduler->millis());
} else if (id == DATA_DISARMED) {
hal.util->set_soft_armed(false);
printf("Disarmed at %lu\n",
(unsigned long)hal.scheduler->millis());
}
dataflash.WriteBlock(msg, f.length);
}

11
Tools/Replay/MsgHandler_Event.h Normal file
View File

@ -0,0 +1,11 @@
#include "MsgHandler.h"
class MsgHandler_Event : public MsgHandler
{
public:
MsgHandler_Event(log_Format &_f, DataFlash_Class &_dataflash,
uint64_t &_last_timestamp_usec)
: MsgHandler(_f, _dataflash, _last_timestamp_usec) { };
virtual void process_message(uint8_t *msg);
};

7
Tools/Replay/Replay.pde
View File

@ -213,11 +213,8 @@ void setup()
ahrs.set_wind_estimation(true);
ahrs.set_correct_centrifugal(true);
if (arm_time_ms != 0) {
hal.util->set_soft_armed(false);
} else {
hal.util->set_soft_armed(true);
}
printf("Starting disarmed\n");
hal.util->set_soft_armed(false);
barometer.init();
barometer.setHIL(0);