diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 083e262de4..9e28aa1666 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -150,7 +150,7 @@ void LogReader::process_plane(uint8_t type, uint8_t *data, uint16_t length) case LOG_PLANE_ATTITUDE_MSG: { struct log_Plane_Attitude msg; if(sizeof(msg) != length) { - printf("Bad ATTITUDE length\n"); + printf("Bad ATTITUDE length %u should be %u\n", (unsigned)length, (unsigned)sizeof(msg)); exit(1); } memcpy(&msg, data, sizeof(msg)); @@ -290,7 +290,9 @@ bool LogReader::update(uint8_t &type) if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) { return false; } - num_formats++; + if (num_formats < LOGREADER_MAX_FORMATS-1) { + num_formats++; + } type = f.type; return true; } diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 772dafaf66..afa04a6835 100644 --- a/Tools/Replay/LogReader.h +++ b/Tools/Replay/LogReader.h @@ -1,9 +1,9 @@ enum log_messages { // plane specific messages - LOG_PLANE_ATTITUDE_MSG = 10, - LOG_PLANE_COMPASS_MSG = 12, - LOG_PLANE_AIRSPEED_MSG = 18, + LOG_PLANE_ATTITUDE_MSG = 9, + LOG_PLANE_COMPASS_MSG = 11, + LOG_PLANE_AIRSPEED_MSG = 17, // copter specific messages LOG_COPTER_ATTITUDE_MSG = 1, @@ -51,8 +51,9 @@ private: uint32_t ground_alt_cm; +#define LOGREADER_MAX_FORMATS 100 uint8_t num_formats; - struct log_Format formats[100]; + struct log_Format formats[LOGREADER_MAX_FORMATS]; Vector3f attitude; Vector3f sim_attitude;