bool LogReader::update(uint8_t &type)
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
if (hdr[2] == LOG_FORMAT_MSG) {
struct log_Format &f = formats[num_formats];
memcpy(&f, hdr, 3);
if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
return false;
}
if (num_formats < LOGREADER_MAX_FORMATS-1) {
num_formats++;
}
type = f.type;
return true;
}
uint8_t i;
for (i=0; i<num_formats; i++) {
if (formats[i].type == hdr[2]) break;
}
if (i == num_formats) {
return false;
}
const struct log_Format &f = formats[i];
uint8_t data[f.length];
memcpy(data, hdr, 3);
if (::read(fd, &data[3], f.length-3) != f.length-3) {
return false;
}
switch (f.type) {
case LOG_MESSAGE_MSG: {
struct log_Message msg;
if(sizeof(msg) != f.length) {
printf("Bad MESSAGE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = VEHICLE_PLANE;
::printf("Detected Plane\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
ahrs.set_fly_forward(true);
} else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) {
vehicle = VEHICLE_COPTER;
::printf("Detected Copter\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
ahrs.set_fly_forward(false);
} else if (strncmp(msg.msg, "ArduRover", strlen("ArduRover")) == 0) {
vehicle = VEHICLE_ROVER;
::printf("Detected Rover\n");
ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
ahrs.set_fly_forward(true);
}
dataflash.Log_Write_Message(msg.msg);
break;
}
case LOG_IMU_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 1) {
ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 1) {
ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
dataflash.Log_Write_IMU(ins);
break;
}
case LOG_IMU2_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU2 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 2) {
ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 2) {
ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
dataflash.Log_Write_IMU(ins);
break;
}
case LOG_IMU3_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU3 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 4) {
ins.set_gyro(2, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 4) {
ins.set_accel(2, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
dataflash.Log_Write_IMU(ins);
break;
}
case LOG_GPS_MSG: {
struct log_GPS msg;
if(sizeof(msg) != f.length) {
printf("Bad GPS length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
Location loc;
loc.lat = msg.latitude;
loc.lng = msg.longitude;
loc.alt = msg.altitude;
loc.options = 0;
Vector3f vel(msg.ground_speed*0.01f*cosf(radians(msg.ground_course*0.01f)),
msg.ground_speed*0.01f*sinf(radians(msg.ground_course*0.01f)),
msg.vel_z);
gps.setHIL(0, (AP_GPS::GPS_Status)msg.status,
msg.apm_time,
loc,
vel,
msg.num_sats,
msg.hdop,
msg.vel_z != 0);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
rel_altitude = msg.rel_altitude*0.01f;
dataflash.Log_Write_GPS(gps, 0, rel_altitude);
break;
}
case LOG_GPS2_MSG: {
struct log_GPS2 msg;
if(sizeof(msg) != f.length) {
printf("Bad GPS2 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
Location loc;
loc.lat = msg.latitude;
loc.lng = msg.longitude;
loc.alt = msg.altitude;
loc.options = 0;
Vector3f vel(msg.ground_speed*0.01f*cosf(radians(msg.ground_course*0.01f)),
msg.ground_speed*0.01f*sinf(radians(msg.ground_course*0.01f)),
msg.vel_z);
gps.setHIL(1, (AP_GPS::GPS_Status)msg.status,
msg.apm_time,
loc,
vel,
msg.num_sats,
msg.hdop,
msg.vel_z != 0);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
dataflash.Log_Write_GPS(gps, 1, rel_altitude);
break;
}
case LOG_SIMSTATE_MSG: {
struct log_AHRS msg;
if(sizeof(msg) != f.length) {
printf("Bad SIMSTATE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
case LOG_BARO_MSG: {
struct log_BARO msg;
if (sizeof(msg) == f.length+sizeof(float)) {
// old style, without climbrate
memset(&msg, 0, sizeof(msg));
memcpy(&msg, data, f.length);
} else if (sizeof(msg) == f.length) {
memcpy(&msg, data, sizeof(msg));
} else {
printf("Bad LOG_BARO length %u expected %u\n",
(unsigned)f.length, (unsigned)sizeof(msg));
exit(1);
}
wait_timestamp(msg.timestamp);
baro.setHIL(0, msg.pressure, msg.temperature*0.01f);
dataflash.Log_Write_Baro(baro);
break;
}
case LOG_PARAMETER_MSG: {
struct log_Parameter msg;
if(sizeof(msg) != f.length) {
printf("Bad LOG_PARAMETER length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
set_parameter(msg.name, msg.value);
break;
}
case LOG_AHR2_MSG: {
struct log_AHRS msg;
if(sizeof(msg) != f.length) {
printf("Bad AHR2 length %u should be %u\n", (unsigned)f.length, (unsigned)sizeof(msg));
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
ahr2_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
default:
if (vehicle == VEHICLE_PLANE) {
process_plane(f.type, data, f.length);
} else if (vehicle == VEHICLE_COPTER) {
process_copter(f.type, data, f.length);
} else if (vehicle == VEHICLE_ROVER) {
process_rover(f.type, data, f.length);
}
break;
}
type = f.type;
return true;
}
bool LogReader::update(uint8_t &type)
{
uint8_t hdr[3];
if (::read(fd, hdr, 3) != 3) {
return false;
}
if (hdr[0] != HEAD_BYTE1 || hdr[1] != HEAD_BYTE2) {
printf("bad log header\n");
return false;
}
if (hdr[2] == LOG_FORMAT_MSG) {
struct log_Format &f = formats[num_formats];
memcpy(&f, hdr, 3);
if (::read(fd, &f.type, sizeof(f)-3) != sizeof(f)-3) {
return false;
}
num_formats++;
type = f.type;
return true;
}
uint8_t i;
for (i=0; i<num_formats; i++) {
if (formats[i].type == hdr[2]) break;
}
if (i == num_formats) {
return false;
}
const struct log_Format &f = formats[i];
uint8_t data[f.length];
memcpy(data, hdr, 3);
if (::read(fd, &data[3], f.length-3) != f.length-3) {
return false;
}
switch (f.type) {
case LOG_MESSAGE_MSG: {
struct log_Message msg;
if(sizeof(msg) != f.length) {
printf("Bad MESSAGE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
if (strncmp(msg.msg, "ArduPlane", strlen("ArduPlane")) == 0) {
vehicle = VEHICLE_PLANE;
::printf("Detected Plane\n");
} else if (strncmp(msg.msg, "ArduCopter", strlen("ArduCopter")) == 0) {
vehicle = VEHICLE_COPTER;
::printf("Detected Copter\n");
} else if (strncmp(msg.msg, "ArduRover", strlen("ArduRover")) == 0) {
vehicle = VEHICLE_ROVER;
::printf("Detected Rover\n");
}
break;
}
case LOG_IMU_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 1) {
ins.set_gyro(0, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 1) {
ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
break;
}
case LOG_IMU2_MSG: {
struct log_IMU msg;
if(sizeof(msg) != f.length) {
printf("Bad IMU2 length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
if (gyro_mask & 2) {
ins.set_gyro(1, Vector3f(msg.gyro_x, msg.gyro_y, msg.gyro_z));
}
if (accel_mask & 2) {
ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
}
break;
}
case LOG_GPS_MSG: {
struct log_GPS msg;
if(sizeof(msg) != f.length) {
printf("Bad GPS length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.apm_time);
gps->setHIL(msg.status==3?GPS::FIX_3D:GPS::FIX_NONE,
msg.apm_time,
msg.latitude*1.0e-7f,
msg.longitude*1.0e-7f,
msg.altitude*1.0e-2f,
msg.ground_speed*1.0e-2f,
msg.ground_course*1.0e-2f,
0,
msg.num_sats);
if (msg.status == 3 && ground_alt_cm == 0) {
ground_alt_cm = msg.altitude;
}
rel_altitude = msg.rel_altitude*0.01f;
break;
}
case LOG_SIMSTATE_MSG: {
struct log_AHRS msg;
if(sizeof(msg) != f.length) {
printf("Bad SIMSTATE length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.time_ms);
sim_attitude = Vector3f(msg.roll*0.01f, msg.pitch*0.01f, msg.yaw*0.01f);
break;
}
case LOG_BARO_MSG: {
struct log_BARO msg;
if(sizeof(msg) != f.length) {
printf("Bad LOG_BARO length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
wait_timestamp(msg.timestamp);
baro.setHIL(msg.pressure, msg.temperature*0.01f);
break;
}
case LOG_PARAMETER_MSG: {
struct log_Parameter msg;
if(sizeof(msg) != f.length) {
printf("Bad LOG_PARAMETER length\n");
exit(1);
}
memcpy(&msg, data, sizeof(msg));
set_parameter(msg.name, msg.value);
break;
}
default:
if (vehicle == VEHICLE_PLANE) {
process_plane(f.type, data, f.length);
} else if (vehicle == VEHICLE_COPTER) {
process_copter(f.type, data, f.length);
} else if (vehicle == VEHICLE_ROVER) {
process_rover(f.type, data, f.length);
}
break;
}
type = f.type;
return true;
}