Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: support raw accel and gyro dataflash logging

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-07 12:08:30 +10:00
parent 21d805422d
commit 7d90033a36
4 changed files with 51 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -284,7 +284,9 @@ AP_InertialSensor::AP_InertialSensor() :
_primary_accel(0),
_hil_mode(false),
_have_3D_calibration(false),
_calibrating(false)
_calibrating(false),
_log_raw_data(false),
_dataflash(NULL)
{
AP_Param::setup_object_defaults(this, var_info);
for (uint8_t i=0; i<INS_MAX_BACKENDS; i++) {

17
libraries/AP_InertialSensor/AP_InertialSensor.h
View File

@ -27,6 +27,12 @@
class AP_InertialSensor_Backend;
/*
forward declare DataFlash class. We can't include DataFlash.h
because of mutual dependencies
*/
class DataFlash_Class;
/* AP_InertialSensor is an abstraction for gyro and accel measurements
* which are correctly aligned to the body axes and scaled to SI units.
*
@ -208,6 +214,12 @@ public:
// get the accel filter rate in Hz
uint8_t get_accel_filter_hz(void) const { return _accel_filter_cutoff; }
// pass in a pointer to DataFlash for raw data logging
void set_dataflash(DataFlash_Class *dataflash) { _dataflash = dataflash; }
// enable/disable raw gyro/accel logging
void set_raw_logging(bool enable) { _log_raw_data = enable; }
private:
// load backend drivers
@ -299,6 +311,9 @@ private:
// are gyros or accels currently being calibrated
bool _calibrating:1;
// should we log raw accel/gyro data?
bool _log_raw_data:1;
// the delta time in seconds for the last sample
float _delta_time;
@ -317,6 +332,8 @@ private:
uint32_t _accel_error_count[INS_MAX_INSTANCES];
uint32_t _gyro_error_count[INS_MAX_INSTANCES];
DataFlash_Class *_dataflash;
};
#include "AP_InertialSensor_Backend.h"

5
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -94,6 +94,11 @@ protected:
// return the requested sample rate in Hz
uint16_t get_sample_rate_hz(void) const;
// access to frontend dataflash
DataFlash_Class *get_dataflash(void) const {
return _imu._log_raw_data? _imu._dataflash : NULL;
}
// note that each backend is also expected to have a static detect()
// function which instantiates an instance of the backend sensor
// driver if the sensor is available

26
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -7,6 +7,8 @@
const extern AP_HAL::HAL& hal;
#include <DataFlash.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
@ -389,6 +391,18 @@ bool AP_InertialSensor_PX4::_get_accel_sample(uint8_t i, struct accel_report &ac
_accel_fd[i] != -1 &&
::read(_accel_fd[i], &accel_report, sizeof(accel_report)) == sizeof(accel_report) &&
accel_report.timestamp != _last_accel_timestamp[i]) {
DataFlash_Class *dataflash = get_dataflash();
if (dataflash != NULL) {
struct log_ACCEL pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ACC1_MSG+i)),
timestamp : (uint32_t)(accel_report.timestamp/1000),
timestamp_us : (uint32_t)accel_report.timestamp,
AccX : accel_report.x,
AccY : accel_report.y,
AccZ : accel_report.z
};
dataflash->WriteBlock(&pkt, sizeof(pkt));
}
return true;
}
return false;
@ -400,6 +414,18 @@ bool AP_InertialSensor_PX4::_get_gyro_sample(uint8_t i, struct gyro_report &gyro
_gyro_fd[i] != -1 &&
::read(_gyro_fd[i], &gyro_report, sizeof(gyro_report)) == sizeof(gyro_report) &&
gyro_report.timestamp != _last_gyro_timestamp[i]) {
DataFlash_Class *dataflash = get_dataflash();
if (dataflash != NULL) {
struct log_GYRO pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GYR1_MSG+i)),
timestamp : (uint32_t)(gyro_report.timestamp/1000),
timestamp_us : (uint32_t)gyro_report.timestamp,
GyrX : gyro_report.x,
GyrY : gyro_report.y,
GyrZ : gyro_report.z
};
dataflash->WriteBlock(&pkt, sizeof(pkt));
}
return true;
}
return false;