Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Replay: fixed FMT messages in replay output

This commit is contained in:
Andrew Tridgell 2016-05-26 09:40:21 +10:00
parent 8e9752c3d3
commit 366d95f45e
3 changed files with 36 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
Tools/Replay/LogReader.cpp
View File

@ -16,6 +16,7 @@
#include <sys/stat.h>
#include "MsgHandler.h"
#include "Replay.h"
#define DEBUG 0
#if DEBUG
@ -28,6 +29,12 @@
extern const AP_HAL::HAL& hal;
static const struct LogStructure log_structure[] = {
LOG_COMMON_STRUCTURES,
{ LOG_CHEK_MSG, sizeof(log_Chek),
"CHEK", "QccCLLffff", "TimeUS,Roll,Pitch,Yaw,Lat,Lng,Alt,VN,VE,VD" }
};
LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps,
AP_Airspeed &_airspeed, DataFlash_Class &_dataflash, const char **&_nottypes):
vehicle(VehicleType::VEHICLE_UNKNOWN),
@ -118,11 +125,9 @@ uint8_t LogReader::map_fmt_type(const char *name, uint8_t intype)
return mapped_msgid[intype];
}
// see if it is in our structure list
uint8_t num_types;
const struct LogStructure *structure = dataflash.get_structures(num_types);
for (uint8_t i=0; i<num_types; i++) {
if (strcmp(name, structure[i].name) == 0) {
mapped_msgid[intype] = structure[i].msg_type;
for (uint8_t i=0; i<ARRAY_SIZE(log_structure); i++) {
if (strcmp(name, log_structure[i].name) == 0) {
mapped_msgid[intype] = log_structure[i].msg_type;
return mapped_msgid[intype];
}
}
@ -329,13 +334,11 @@ bool LogReader::set_parameter(const char *name, float value)
*/
void LogReader::end_format_msgs(void)
{
uint8_t num_types;
const struct LogStructure *structure = dataflash.get_structures(num_types);
// write out any formats we will be producing
for (uint8_t i=0; generated_names[i]; i++) {
for (uint8_t n=0; n<num_types; n++) {
if (strcmp(generated_names[i], structure[n].name) == 0) {
const struct LogStructure *s = &structure[n];
for (uint8_t n=0; n<ARRAY_SIZE(log_structure); n++) {
if (strcmp(generated_names[i], log_structure[n].name) == 0) {
const struct LogStructure *s = &log_structure[n];
struct log_Format pkt {};
pkt.head1 = HEAD_BYTE1;
pkt.head2 = HEAD_BYTE2;

30
Tools/Replay/Replay.cpp
View File

@ -98,41 +98,13 @@ void ReplayVehicle::load_parameters(void)
AP_Param::set_default_by_name("LOG_FILE_BUFSIZE", 60);
}
/*
Replay specific log structures
*/
struct PACKED log_Chek {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t roll;
int16_t pitch;
uint16_t yaw;
int32_t lat;
int32_t lng;
float alt;
float vnorth;
float veast;
float vdown;
};
enum {
LOG_CHEK_MSG=100
};
static const struct LogStructure log_structure[] = {
LOG_COMMON_STRUCTURES,
{ LOG_CHEK_MSG, sizeof(log_Chek),
"CHEK", "QccCLLffff", "TimeUS,Roll,Pitch,Yaw,Lat,Lng,Alt,VN,VE,VD" }
};
void ReplayVehicle::setup(void)
{
load_parameters();
// we pass zero log structures, as we will be outputting the log
// structures we need manually, to prevent FMT duplicates
dataflash.Init(log_structure, 0);
dataflash.Init(nullptr, 0);
dataflash.StartNewLog();
ahrs.set_compass(&compass);

22
Tools/Replay/Replay.h
View File

@ -177,4 +177,26 @@ private:
void load_param_file(const char *filename);
};
enum {
LOG_CHEK_MSG=100
};
/*
Replay specific log structures
*/
struct PACKED log_Chek {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t roll;
int16_t pitch;
uint16_t yaw;
int32_t lat;
int32_t lng;
float alt;
float vnorth;
float veast;
float vdown;
};
extern Replay replay;