mirror of https://github.com/ArduPilot/ardupilot
Replay: allow replay on ChibiOS
This commit is contained in:
Andrew Tridgell
parent
5e8380ff41
commit
4edc784dc4
|
|
@ -1,4 +1,5 @@
|
|||
#include "DataFlashFileReader.h"
|
||||
#include <AP_Filesystem/AP_Filesystem.h>
|
||||
|
||||
#include <fcntl.h>
|
||||
#include <string.h>
|
||||
|
|
@ -12,28 +13,17 @@
|
|||
#define PRIu64 "llu"
|
||||
#endif
|
||||
|
||||
// flogged from AP_Hal_Linux/system.cpp; we don't want to use stopped clock here
|
||||
uint64_t now() {
|
||||
struct timespec ts;
|
||||
clock_gettime(CLOCK_MONOTONIC, &ts);
|
||||
return 1.0e6*((ts.tv_sec + (ts.tv_nsec*1.0e-9)));
|
||||
}
|
||||
|
||||
AP_LoggerFileReader::AP_LoggerFileReader() :
|
||||
start_micros(now())
|
||||
AP_LoggerFileReader::AP_LoggerFileReader()
|
||||
{}
|
||||
|
||||
AP_LoggerFileReader::~AP_LoggerFileReader()
|
||||
{
|
||||
const uint64_t micros = now();
|
||||
const uint64_t delta = micros - start_micros;
|
||||
::printf("Replay counts: %" PRIu64 " bytes %u entries\n", bytes_read, message_count);
|
||||
::printf("Replay rates: %" PRIu64 " bytes/second %" PRIu64 " messages/second\n", bytes_read*1000000/delta, message_count*1000000/delta);
|
||||
}
|
||||
|
||||
bool AP_LoggerFileReader::open_log(const char *logfile)
|
||||
{
|
||||
fd = ::open(logfile, O_RDONLY|O_CLOEXEC);
|
||||
fd = AP::FS().open(logfile, O_RDONLY);
|
||||
if (fd == -1) {
|
||||
return false;
|
||||
}
|
||||
|
|
@ -42,7 +32,7 @@ bool AP_LoggerFileReader::open_log(const char *logfile)
|
|||
|
||||
ssize_t AP_LoggerFileReader::read_input(void *buffer, const size_t count)
|
||||
{
|
||||
uint64_t ret = ::read(fd, buffer, count);
|
||||
uint64_t ret = AP::FS().read(fd, buffer, count);
|
||||
bytes_read += ret;
|
||||
return ret;
|
||||
}
|
||||
|
|
@ -72,6 +62,12 @@ bool AP_LoggerFileReader::update()
|
|||
return false;
|
||||
}
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||
// running on stm32 is slow enough it is nice to see progress
|
||||
if (message_count % 500 == 0) {
|
||||
::printf("line %u pkt 0x%02x t=%u\n", message_count, hdr[2], AP_HAL::millis());
|
||||
}
|
||||
#endif
|
||||
packet_counts[hdr[2]]++;
|
||||
|
||||
if (hdr[2] == LOG_FORMAT_MSG) {
|
||||
|
|
|
|||
|
|
@ -2,39 +2,41 @@
|
|||
#include "LogReader.h"
|
||||
#include "Replay.h"
|
||||
|
||||
#include <AP_HAL_Linux/Scheduler.h>
|
||||
#include <AP_DAL/AP_DAL.h>
|
||||
|
||||
#include <cinttypes>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define MSG_CAST(sname,msg) *((log_ ##sname *)(msg+3))
|
||||
#define MSG_CREATE(sname,msgbytes) log_ ##sname msg; memcpy((void*)&msg, (msgbytes)+3, sizeof(msg));
|
||||
|
||||
LR_MsgHandler::LR_MsgHandler(struct log_Format &_f) :
|
||||
MsgHandler(_f) {
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RFRH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RFRH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RFRH,msg));
|
||||
MSG_CREATE(RFRH, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RFRF::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RFRF::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RFRF,msg), ekf2, ekf3);
|
||||
MSG_CREATE(RFRF, msgbytes);
|
||||
AP::dal().handle_message(msg, ekf2, ekf3);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RFRN::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RFRN::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RFRN,msg));
|
||||
MSG_CREATE(RFRN, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_REV2::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_REV2::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
const log_REV2 &rev2 = MSG_CAST(REV2,msg);
|
||||
MSG_CREATE(REV2, msgbytes);
|
||||
|
||||
switch ((AP_DAL::Event2)rev2.event) {
|
||||
switch ((AP_DAL::Event2)msg.event) {
|
||||
|
||||
case AP_DAL::Event2::resetGyroBias:
|
||||
ekf2.resetGyroBias();
|
||||
|
|
@ -78,28 +80,28 @@ void LR_MsgHandler_REV2::process_message(uint8_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RSO2::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RSO2::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
const log_RSO2 &rso2 = MSG_CAST(RSO2,msg);
|
||||
MSG_CREATE(RSO2, msgbytes);
|
||||
Location loc;
|
||||
loc.lat = rso2.lat;
|
||||
loc.lng = rso2.lng;
|
||||
loc.alt = rso2.alt;
|
||||
loc.lat = msg.lat;
|
||||
loc.lng = msg.lng;
|
||||
loc.alt = msg.alt;
|
||||
ekf2.setOriginLLH(loc);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RWA2::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RWA2::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
const log_RWA2 &rwa2 = MSG_CAST(RWA2,msg);
|
||||
ekf2.writeDefaultAirSpeed(rwa2.airspeed);
|
||||
MSG_CREATE(RWA2, msgbytes);
|
||||
ekf2.writeDefaultAirSpeed(msg.airspeed);
|
||||
}
|
||||
|
||||
|
||||
void LR_MsgHandler_REV3::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_REV3::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
const log_REV3 &rev3 = MSG_CAST(REV3,msg);
|
||||
MSG_CREATE(REV3, msgbytes);
|
||||
|
||||
switch ((AP_DAL::Event3)rev3.event) {
|
||||
switch ((AP_DAL::Event3)msg.event) {
|
||||
|
||||
case AP_DAL::Event3::resetGyroBias:
|
||||
ekf3.resetGyroBias();
|
||||
|
|
@ -143,123 +145,150 @@ void LR_MsgHandler_REV3::process_message(uint8_t *msg)
|
|||
}
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RSO3::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RSO3::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
const log_RSO3 &rso3 = MSG_CAST(RSO3,msg);
|
||||
MSG_CREATE(RSO3, msgbytes);
|
||||
Location loc;
|
||||
loc.lat = rso3.lat;
|
||||
loc.lng = rso3.lng;
|
||||
loc.alt = rso3.alt;
|
||||
loc.lat = msg.lat;
|
||||
loc.lng = msg.lng;
|
||||
loc.alt = msg.alt;
|
||||
ekf3.setOriginLLH(loc);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RWA3::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RWA3::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
const log_RWA3 &rwa3 = MSG_CAST(RWA3,msg);
|
||||
ekf3.writeDefaultAirSpeed(rwa3.airspeed);
|
||||
MSG_CREATE(RWA3, msgbytes);
|
||||
ekf3.writeDefaultAirSpeed(msg.airspeed);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_REY3::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_REY3::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
const log_REY3 &rey3 = MSG_CAST(REY3,msg);
|
||||
ekf3.writeEulerYawAngle(rey3.yawangle, rey3.yawangleerr, rey3.timestamp_ms, rey3.type);
|
||||
MSG_CREATE(REY3, msgbytes);
|
||||
ekf3.writeEulerYawAngle(msg.yawangle, msg.yawangleerr, msg.timestamp_ms, msg.type);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RISH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RISH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RISH,msg));
|
||||
MSG_CREATE(RISH, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
void LR_MsgHandler_RISI::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RISI::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RISI,msg));
|
||||
MSG_CREATE(RISI, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RASH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RASH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RASH,msg));
|
||||
MSG_CREATE(RASH, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
void LR_MsgHandler_RASI::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RASI::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RASI,msg));
|
||||
MSG_CREATE(RASI, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RBRH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RBRH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RBRH,msg));
|
||||
}
|
||||
void LR_MsgHandler_RBRI::process_message(uint8_t *msg)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RBRI,msg));
|
||||
MSG_CREATE(RBRH, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RRNH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RBRI::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RRNH,msg));
|
||||
}
|
||||
void LR_MsgHandler_RRNI::process_message(uint8_t *msg)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RRNI,msg));
|
||||
MSG_CREATE(RBRI, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RGPH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RRNH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RGPH,msg));
|
||||
}
|
||||
void LR_MsgHandler_RGPI::process_message(uint8_t *msg)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RGPI,msg));
|
||||
}
|
||||
void LR_MsgHandler_RGPJ::process_message(uint8_t *msg)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RGPJ,msg));
|
||||
MSG_CREATE(RRNH, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RMGH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RRNI::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RMGH,msg));
|
||||
}
|
||||
void LR_MsgHandler_RMGI::process_message(uint8_t *msg)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RMGI,msg));
|
||||
MSG_CREATE(RRNI, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RBCH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RGPH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RBCH,msg));
|
||||
}
|
||||
void LR_MsgHandler_RBCI::process_message(uint8_t *msg)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RBCI,msg));
|
||||
MSG_CREATE(RGPH, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RVOH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RGPI::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RVOH,msg));
|
||||
MSG_CREATE(RGPI, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_ROFH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RGPJ::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(ROFH,msg), ekf2, ekf3);
|
||||
MSG_CREATE(RGPJ, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RWOH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RMGH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RWOH,msg), ekf2, ekf3);
|
||||
MSG_CREATE(RMGH, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RBOH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RMGI::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(RBOH,msg), ekf2, ekf3);
|
||||
MSG_CREATE(RMGI, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_REPH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RBCH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(REPH,msg), ekf2, ekf3);
|
||||
MSG_CREATE(RBCH, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_REVH::process_message(uint8_t *msg)
|
||||
void LR_MsgHandler_RBCI::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
AP::dal().handle_message(MSG_CAST(REVH,msg), ekf2, ekf3);
|
||||
MSG_CREATE(RBCI, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RVOH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
MSG_CREATE(RVOH, msgbytes);
|
||||
AP::dal().handle_message(msg);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_ROFH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
MSG_CREATE(ROFH, msgbytes);
|
||||
AP::dal().handle_message(msg, ekf2, ekf3);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RWOH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
MSG_CREATE(RWOH, msgbytes);
|
||||
AP::dal().handle_message(msg, ekf2, ekf3);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_RBOH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
MSG_CREATE(RBOH, msgbytes);
|
||||
AP::dal().handle_message(msg, ekf2, ekf3);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_REPH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
MSG_CREATE(REPH, msgbytes);
|
||||
AP::dal().handle_message(msg, ekf2, ekf3);
|
||||
}
|
||||
|
||||
void LR_MsgHandler_REVH::process_message(uint8_t *msgbytes)
|
||||
{
|
||||
MSG_CREATE(REVH, msgbytes);
|
||||
AP::dal().handle_message(msg, ekf2, ekf3);
|
||||
}
|
||||
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
|
|
@ -278,24 +307,13 @@ bool LR_MsgHandler_PARM::set_parameter(const char *name, const float value)
|
|||
}
|
||||
}
|
||||
|
||||
return _set_parameter_callback(name, value);
|
||||
return LogReader::set_parameter(name, value);
|
||||
}
|
||||
|
||||
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)) {
|
||||
} 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
|
||||
// AP_Logger's IO only when stop_clock is called, we can
|
||||
// overflow AP_Logger's ringbuffer. This should force us to
|
||||
// do IO:
|
||||
hal.scheduler->stop_clock(((Linux::Scheduler*)hal.scheduler)->stopped_clock_usec());
|
||||
}
|
||||
|
||||
require_field(msg, "Name", parameter_name, parameter_name_len);
|
||||
|
||||
|
|
|
|||
|
|
@ -6,8 +6,6 @@
|
|||
#include <AP_NavEKF2/AP_NavEKF2.h>
|
||||
#include <AP_NavEKF3/AP_NavEKF3.h>
|
||||
|
||||
#include <functional>
|
||||
|
||||
class LR_MsgHandler : public MsgHandler {
|
||||
public:
|
||||
LR_MsgHandler(struct log_Format &f);
|
||||
|
|
@ -262,15 +260,12 @@ public:
|
|||
class LR_MsgHandler_PARM : public LR_MsgHandler
|
||||
{
|
||||
public:
|
||||
LR_MsgHandler_PARM(log_Format &_f,
|
||||
const std::function<bool(const char *name, const float)>&set_parameter_callback) :
|
||||
LR_MsgHandler(_f),
|
||||
_set_parameter_callback(set_parameter_callback)
|
||||
LR_MsgHandler_PARM(log_Format &_f) :
|
||||
LR_MsgHandler(_f)
|
||||
{};
|
||||
|
||||
void process_message(uint8_t *msg) override;
|
||||
|
||||
private:
|
||||
bool set_parameter(const char *name, const float value);
|
||||
const std::function<bool(const char *name, const float)>_set_parameter_callback;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -73,7 +73,6 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
|
|||
return mapped_msgid[intype];
|
||||
}
|
||||
for (uint8_t n=next_msgid; n<255; n++) {
|
||||
// ::fprintf(stderr, "next_msgid=%u\n", n);
|
||||
bool already_mapped = false;
|
||||
for (uint16_t i=0; i<sizeof(mapped_msgid); i++) {
|
||||
if (mapped_msgid[i] == n) {
|
||||
|
|
@ -97,14 +96,14 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
|
|||
break;
|
||||
}
|
||||
if (mapped_msgid[intype] == 0) {
|
||||
::fprintf(stderr, "mapping failed\n");
|
||||
::printf("mapping failed\n");
|
||||
abort();
|
||||
}
|
||||
|
||||
return mapped_msgid[intype];
|
||||
}
|
||||
|
||||
bool LogReader::handle_log_format_msg(const struct log_Format &f)
|
||||
bool LogReader::handle_log_format_msg(const struct log_Format &f)
|
||||
{
|
||||
// emit the output as we receive it:
|
||||
AP::logger().WriteBlock((void*)&f, sizeof(f));
|
||||
|
|
@ -144,14 +143,10 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
|
|||
return true;
|
||||
}
|
||||
|
||||
// map from format name to a parser subclass:
|
||||
// map from format name to a parser subclass:
|
||||
if (streq(name, "PARM")) {
|
||||
msgparser[f.type] = new LR_MsgHandler_PARM
|
||||
(formats[f.type],
|
||||
[this](const char *xname, const float xvalue) {
|
||||
return set_parameter(xname, xvalue);
|
||||
});
|
||||
} else if (streq(name, "RFRH")) {
|
||||
msgparser[f.type] = new LR_MsgHandler_PARM(formats[f.type]);
|
||||
} else if (streq(name, "RFRH")) {
|
||||
msgparser[f.type] = new LR_MsgHandler_RFRH(formats[f.type]);
|
||||
} else if (streq(name, "RFRF")) {
|
||||
msgparser[f.type] = new LR_MsgHandler_RFRF(formats[f.type], ekf2, ekf3);
|
||||
|
|
@ -258,7 +253,6 @@ bool LogReader::set_parameter(const char *name, float value, bool force)
|
|||
if (vp == NULL) {
|
||||
// a lot of parameters will not be found - e.g. FORMAT_VERSION
|
||||
// and all of the vehicle-specific parameters, ....
|
||||
// ::fprintf(stderr, "Parameter (%s) not found\n", name);
|
||||
return false;
|
||||
}
|
||||
float old_value = 0;
|
||||
|
|
@ -278,7 +272,8 @@ bool LogReader::set_parameter(const char *name, float value, bool force)
|
|||
AP_HAL::panic("What manner of evil is var_type=%u", var_type);
|
||||
}
|
||||
if (fabsf(old_value - value) > 1.0e-12) {
|
||||
::fprintf(stderr, "Changed %s to %.8f from %.8f\n", name, value, old_value);
|
||||
::printf("Changed %s to %.8f from %.8f\n", name, value, old_value);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -12,8 +12,8 @@ public:
|
|||
|
||||
VehicleType::vehicle_type vehicle;
|
||||
|
||||
bool check_user_param(const char *name);
|
||||
bool set_parameter(const char *name, float value, bool force=false);
|
||||
static bool check_user_param(const char *name);
|
||||
static bool set_parameter(const char *name, float value, bool force=false);
|
||||
|
||||
bool handle_log_format_msg(const struct log_Format &f) override;
|
||||
bool handle_msg(const struct log_Format &f, uint8_t *msg) override;
|
||||
|
|
|
|||
|
|
@ -1,31 +1,5 @@
|
|||
#include "MsgHandler.h"
|
||||
|
||||
void fatal(const char *msg) {
|
||||
::printf("%s",msg);
|
||||
::printf("\n");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
char *xstrdup(const char *string)
|
||||
{
|
||||
char *ret = strdup(string);
|
||||
if (ret == NULL) {
|
||||
perror("strdup");
|
||||
fatal("strdup failed");
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
char *xcalloc(uint8_t count, uint32_t len)
|
||||
{
|
||||
char *ret = (char*)calloc(count, len);
|
||||
if (ret == nullptr) {
|
||||
perror("calloc");
|
||||
fatal("calloc failed");
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
void MsgHandler::add_field_type(char type, size_t size)
|
||||
{
|
||||
size_for_type_table[(type > 'A' ? (type-'A') : (type-'a'))] = size;
|
||||
|
|
@ -35,7 +9,7 @@ uint8_t MsgHandler::size_for_type(char type)
|
|||
{
|
||||
uint8_t ret = size_for_type_table[(uint8_t)(type > 'A' ? (type-'A') : (type-'a'))];
|
||||
if (ret == 0) {
|
||||
::fprintf(stderr, "Unknown type (%c)\n", type);
|
||||
::printf("Unknown type (%c)\n", type);
|
||||
abort();
|
||||
}
|
||||
return ret;
|
||||
|
|
@ -83,16 +57,16 @@ MsgHandler::MsgHandler(const struct log_Format &_f) : next_field(0), f(_f)
|
|||
void MsgHandler::add_field(const char *_label, uint8_t _type, uint8_t _offset,
|
||||
uint8_t _length)
|
||||
{
|
||||
field_info[next_field].label = xstrdup(_label);
|
||||
field_info[next_field].label = strdup(_label);
|
||||
field_info[next_field].type = _type;
|
||||
field_info[next_field].offset = _offset;
|
||||
field_info[next_field].length = _length;
|
||||
next_field++;
|
||||
}
|
||||
|
||||
char *get_string_field(char *field, uint8_t fieldlen)
|
||||
static char *get_string_field(char *field, uint8_t fieldlen)
|
||||
{
|
||||
char *ret = xcalloc(1, fieldlen+1);
|
||||
char *ret = (char *)malloc(fieldlen+1);
|
||||
memcpy(ret, field, fieldlen);
|
||||
return ret;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,8 +23,12 @@
|
|||
#include <AP_Vehicle/AP_Vehicle.h>
|
||||
|
||||
#include <GCS_MAVLink/GCS_Dummy.h>
|
||||
#include <AP_Filesystem/AP_Filesystem.h>
|
||||
#include <AP_Filesystem/posix_compat.h>
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX
|
||||
#include <AP_HAL_Linux/Scheduler.h>
|
||||
#endif
|
||||
|
||||
#define streq(x, y) (!strcmp(x, y))
|
||||
|
||||
|
|
@ -194,12 +198,17 @@ void Replay::setup()
|
|||
set_user_parameters();
|
||||
|
||||
if (filename == nullptr) {
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||
// allow replay on stm32
|
||||
filename = "APM/replayin.bin";
|
||||
#else
|
||||
::printf("You must supply a log filename\n");
|
||||
exit(1);
|
||||
#endif
|
||||
}
|
||||
// LogReader reader = LogReader(log_structure);
|
||||
if (!reader.open_log(filename)) {
|
||||
::fprintf(stderr, "open(%s): %m\n", filename);
|
||||
::printf("open(%s): %m\n", filename);
|
||||
exit(1);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4,9 +4,6 @@
|
|||
import boards
|
||||
|
||||
def build(bld):
|
||||
if isinstance(bld.get_board(), boards.chibios):
|
||||
return
|
||||
|
||||
vehicle = bld.path.name
|
||||
|
||||
bld.ap_stlib(
|
||||
|
|
|
|||
Loading…
Reference in New Issue