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

Replay: fixed trailing nul and don't wait for EKF to init

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-11 11:16:05 +10:00
parent 6e97f2e1fb
commit 64a55c908f
3 changed files with 16 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Tools/Replay/LogReader.cpp
View File

@ -94,7 +94,7 @@ void LogReader::maybe_install_vehicle_specific_parsers() {
MsgHandler_PARM *parameter_handler;
bool LogReader::update(const char **type)
bool LogReader::update(char type[5])
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
@ -112,7 +112,8 @@ bool LogReader::update(const char **type)
return false;
}
memcpy(&formats[f.type], &f, sizeof(formats[f.type]));
*type = f.name;
strncpy(type, f.name, 4);
type[4] = 0;
char name[5];
memset(name, '\0', 5);
@ -204,7 +205,8 @@ bool LogReader::update(const char **type)
return false;
}
*type = f.name;
strncpy(type, f.name, 4);
type[4] = 0;
MsgHandler *p = msgparser[f.type];
if (p == NULL) {
@ -224,8 +226,8 @@ bool LogReader::update(const char **type)
bool LogReader::wait_type(const char *wtype)
{
while (true) {
const char *type;
if (!update(&type)) {
char type[5];
if (!update(type)) {
return false;
}
if (streq(type,wtype)) {

2
Tools/Replay/LogReader.h
View File

@ -5,7 +5,7 @@ class LogReader
public:
LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, Compass &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash);
bool open_log(const char *logfile);
bool update(const char **type);
bool update(char type[5]);
bool wait_type(const char *type);
const Vector3f &get_attitude(void) const { return attitude; }

21
Tools/Replay/Replay.pde
View File

@ -257,10 +257,12 @@ void setup()
ahrs.set_ekf_use(true);
::printf("Waiting for InertialNav to start\n");
while (!ahrs.have_inertial_nav()) {
const char *type;
if (!LogReader.update(&type)) break;
::printf("Waiting for GPS\n");
while (!done_home_init) {
char type[5];
if (!LogReader.update(type)) {
break;
}
read_sensors(type);
if (streq(type, "GPS") &&
gps.status() >= AP_GPS::GPS_OK_FIX_3D &&
@ -276,13 +278,6 @@ void setup()
done_home_init = true;
}
}
::printf("InertialNav started\n");
if (!ahrs.have_inertial_nav()) {
::printf("Failed to start NavEKF\n");
exit(1);
}
}
@ -364,7 +359,7 @@ static void read_sensors(const char *type)
void loop()
{
while (true) {
const char *type;
char type[5];
if (arm_time_ms != 0 && hal.scheduler->millis() > arm_time_ms) {
if (!hal.util->get_soft_armed()) {
@ -373,7 +368,7 @@ void loop()
}
}
if (!LogReader.update(&type)) {
if (!LogReader.update(type)) {
::printf("End of log at %.1f seconds\n", hal.scheduler->millis()*0.001f);
fclose(plotf);
exit(0);