2013-12-29 07:56:54 -04:00
|
|
|
|
|
|
|
enum log_messages {
|
2014-01-04 20:39:43 -04:00
|
|
|
// plane specific messages
|
2014-04-20 08:45:31 -03:00
|
|
|
LOG_PLANE_ATTITUDE_MSG = 9,
|
|
|
|
LOG_PLANE_COMPASS_MSG = 11,
|
2014-10-14 19:17:22 -03:00
|
|
|
LOG_PLANE_COMPASS2_MSG = 15,
|
2014-04-20 08:45:31 -03:00
|
|
|
LOG_PLANE_AIRSPEED_MSG = 17,
|
2014-01-04 20:39:43 -04:00
|
|
|
|
|
|
|
// copter specific messages
|
|
|
|
LOG_COPTER_ATTITUDE_MSG = 1,
|
|
|
|
LOG_COPTER_COMPASS_MSG = 15,
|
2014-02-25 05:56:53 -04:00
|
|
|
LOG_COPTER_NAV_TUNING_MSG = 5,
|
2014-03-06 02:50:50 -04:00
|
|
|
|
|
|
|
// rover specific messages
|
|
|
|
LOG_ROVER_ATTITUDE_MSG = 8,
|
|
|
|
LOG_ROVER_COMPASS_MSG = 10,
|
2013-12-29 07:56:54 -04:00
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
class LogReader
|
|
|
|
{
|
|
|
|
public:
|
2015-01-05 07:28:06 -04:00
|
|
|
LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro &_baro, AP_Compass_HIL &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash);
|
2013-12-29 07:56:54 -04:00
|
|
|
bool open_log(const char *logfile);
|
|
|
|
bool update(uint8_t &type);
|
|
|
|
bool wait_type(uint8_t type);
|
2014-01-05 01:37:47 -04:00
|
|
|
|
2013-12-29 07:56:54 -04:00
|
|
|
const Vector3f &get_attitude(void) const { return attitude; }
|
2014-10-14 19:17:22 -03:00
|
|
|
const Vector3f &get_ahr2_attitude(void) const { return ahr2_attitude; }
|
2014-01-05 01:37:47 -04:00
|
|
|
const Vector3f &get_inavpos(void) const { return inavpos; }
|
2014-01-03 01:07:39 -04:00
|
|
|
const Vector3f &get_sim_attitude(void) const { return sim_attitude; }
|
2014-02-22 17:17:55 -04:00
|
|
|
const float &get_relalt(void) const { return rel_altitude; }
|
2013-12-29 07:56:54 -04:00
|
|
|
|
2014-01-04 20:39:43 -04:00
|
|
|
enum vehicle_type { VEHICLE_UNKNOWN, VEHICLE_COPTER, VEHICLE_PLANE, VEHICLE_ROVER };
|
|
|
|
|
|
|
|
vehicle_type vehicle;
|
|
|
|
|
2014-03-01 00:15:46 -04:00
|
|
|
bool set_parameter(const char *name, float value);
|
|
|
|
|
2014-03-01 17:00:13 -04:00
|
|
|
void set_accel_mask(uint8_t mask) { accel_mask = mask; }
|
|
|
|
void set_gyro_mask(uint8_t mask) { gyro_mask = mask; }
|
|
|
|
|
2013-12-29 07:56:54 -04:00
|
|
|
private:
|
|
|
|
int fd;
|
2014-04-21 05:11:20 -03:00
|
|
|
AP_AHRS &ahrs;
|
2013-12-29 07:56:54 -04:00
|
|
|
AP_InertialSensor &ins;
|
2015-01-05 07:28:06 -04:00
|
|
|
AP_Baro &baro;
|
2013-12-29 07:56:54 -04:00
|
|
|
AP_Compass_HIL &compass;
|
2014-03-31 04:46:01 -03:00
|
|
|
AP_GPS &gps;
|
2014-02-17 18:11:46 -04:00
|
|
|
AP_Airspeed &airspeed;
|
2014-12-07 20:25:22 -04:00
|
|
|
DataFlash_Class &dataflash;
|
2013-12-29 07:56:54 -04:00
|
|
|
|
2014-03-01 17:00:13 -04:00
|
|
|
uint8_t accel_mask;
|
|
|
|
uint8_t gyro_mask;
|
|
|
|
|
2013-12-29 18:32:20 -04:00
|
|
|
uint32_t ground_alt_cm;
|
|
|
|
|
2014-04-20 08:45:31 -03:00
|
|
|
#define LOGREADER_MAX_FORMATS 100
|
2013-12-29 07:56:54 -04:00
|
|
|
uint8_t num_formats;
|
2014-04-20 08:45:31 -03:00
|
|
|
struct log_Format formats[LOGREADER_MAX_FORMATS];
|
2013-12-29 07:56:54 -04:00
|
|
|
|
|
|
|
Vector3f attitude;
|
2014-10-14 19:17:22 -03:00
|
|
|
Vector3f ahr2_attitude;
|
2014-01-03 01:07:39 -04:00
|
|
|
Vector3f sim_attitude;
|
2014-01-05 01:37:47 -04:00
|
|
|
Vector3f inavpos;
|
2014-02-22 17:17:55 -04:00
|
|
|
float rel_altitude;
|
2013-12-29 07:56:54 -04:00
|
|
|
|
|
|
|
void wait_timestamp(uint32_t timestamp);
|
2014-01-04 20:39:43 -04:00
|
|
|
|
|
|
|
void process_plane(uint8_t type, uint8_t *data, uint16_t length);
|
|
|
|
void process_copter(uint8_t type, uint8_t *data, uint16_t length);
|
2014-03-06 02:50:50 -04:00
|
|
|
void process_rover(uint8_t type, uint8_t *data, uint16_t length);
|
2013-12-29 07:56:54 -04:00
|
|
|
};
|