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

AP_InertialSensor: moved raw gyro and accel logging to common code

Browse Source 
this brings raw logging to non-PX4 ports
This commit is contained in:
Andrew Tridgell 2015-11-16 11:58:08 +11:00
parent 75ea8f3dc0
commit ccd915eb67
8 changed files with 40 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -3,6 +3,7 @@
#include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
#include <DataFlash/DataFlash.h>
const extern AP_HAL::HAL& hal;
@ -57,7 +58,8 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
}
void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
const Vector3f &gyro)
const Vector3f &gyro,
uint64_t sample_us)
{
float dt;
@ -93,6 +95,20 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
_imu._gyro_filtered[instance] = _imu._gyro_filter[instance].apply(gyro);
_imu._new_gyro_data[instance] = true;
DataFlash_Class *dataflash = get_dataflash();
if (dataflash != NULL) {
uint64_t now = hal.scheduler->micros64();
struct log_GYRO pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_GYR1_MSG+instance)),
time_us : now,
sample_us : sample_us?sample_us:now,
GyrX : gyro.x,
GyrY : gyro.y,
GyrZ : gyro.z
};
dataflash->WriteBlock(&pkt, sizeof(pkt));
}
}
/*
@ -114,7 +130,8 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f
}
void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
const Vector3f &accel)
const Vector3f &accel,
uint64_t sample_us)
{
float dt;
@ -133,6 +150,20 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
_imu._accel_filtered[instance] = _imu._accel_filter[instance].apply(accel);
_imu._new_accel_data[instance] = true;
DataFlash_Class *dataflash = get_dataflash();
if (dataflash != NULL) {
uint64_t now = hal.scheduler->micros64();
struct log_ACCEL pkt = {
LOG_PACKET_HEADER_INIT((uint8_t)(LOG_ACC1_MSG+instance)),
time_us : now,
sample_us : sample_us?sample_us:now,
AccX : accel.x,
AccY : accel.y,
AccZ : accel.z
};
dataflash->WriteBlock(&pkt, sizeof(pkt));
}
}
void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance,

4
libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
View File

@ -80,7 +80,7 @@ protected:
// be it published or not
// the sample is raw in the sense that it's not filtered yet, but it must
// be rotated and corrected (_rotate_and_correct_gyro)
void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel);
void _notify_new_gyro_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0);
// rotate accel vector, scale, offset and publish
void _publish_accel(uint8_t instance, const Vector3f &accel);
@ -89,7 +89,7 @@ protected:
// be it published or not
// the sample is raw in the sense that it's not filtered yet, but it must
// be rotated and corrected (_rotate_and_correct_accel)
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel);
void _notify_new_accel_raw_sample(uint8_t instance, const Vector3f &accel, uint64_t sample_us=0);
// set accelerometer max absolute offset for calibration
void _set_accel_max_abs_offset(uint8_t instance, float offset);

8
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp
View File

@ -65,8 +65,6 @@ const uint32_t raw_sample_interval_us = (1000000 / raw_sample_rate_hz);
AP_InertialSensor_Flymaple::AP_InertialSensor_Flymaple(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_have_gyro_sample(false),
_have_accel_sample(false),
_accel_filter(raw_sample_rate_hz, 10),
_gyro_filter(raw_sample_rate_hz, 10),
_last_gyro_timestamp(0),
@ -166,9 +164,6 @@ void AP_InertialSensor_Flymaple::_set_filter_frequency(uint8_t filter_hz)
// This takes about 20us to run
bool AP_InertialSensor_Flymaple::update(void)
{
_have_gyro_sample = false;
_have_accel_sample = false;
update_accel(_accel_instance);
update_gyro(_gyro_instance);
@ -215,7 +210,6 @@ void AP_InertialSensor_Flymaple::accumulate(void)
accel *= FLYMAPLE_ACCELEROMETER_SCALE_M_S;
_rotate_and_correct_accel(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel);
_have_accel_sample = true;
_last_accel_timestamp = now;
}
@ -234,8 +228,6 @@ void AP_InertialSensor_Flymaple::accumulate(void)
gyro *= FLYMAPLE_GYRO_SCALE_R_S;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_have_gyro_sample = true;
_last_gyro_timestamp = now;
}
// give back i2c semaphore

6
libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.h
View File

@ -26,14 +26,10 @@ public:
private:
bool _init_sensor(void);
bool _have_gyro_sample;
bool _have_accel_sample;
uint8_t _gyro_instance;
uint8_t _accel_instance;
uint32_t _last_gyro_timestamp;
uint32_t _last_accel_timestamp;
};
#endif
#endif // __AP_INERTIAL_SENSOR_FLYMAPLE_H__

11
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.cpp
View File

@ -113,9 +113,7 @@ const extern AP_HAL::HAL& hal;
// constructor
AP_InertialSensor_L3G4200D::AP_InertialSensor_L3G4200D(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_have_gyro_sample(false),
_have_accel_sample(false)
AP_InertialSensor_Backend(imu)
{
}
@ -289,7 +287,6 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
gyro *= L3G4200D_GYRO_SCALE_R_S;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_have_gyro_sample = true;
}
}
}
@ -313,18 +310,12 @@ void AP_InertialSensor_L3G4200D::_accumulate(void)
accel *= ADXL345_ACCELEROMETER_SCALE_M_S;
_rotate_and_correct_accel(_accel_instance, accel);
_notify_new_accel_raw_sample(_accel_instance, accel);
_have_accel_sample = true;
}
}
}
// give back i2c semaphore
i2c_sem->give();
if (_have_accel_sample && _have_gyro_sample) {
_have_gyro_sample = false;
_have_accel_sample = false;
}
}
#endif // CONFIG_HAL_BOARD

3
libraries/AP_InertialSensor/AP_InertialSensor_L3G4200D.h
View File

@ -32,9 +32,6 @@ private:
bool _init_sensor(void);
void _accumulate(void);
bool _have_gyro_sample;
bool _have_accel_sample;
// gyro and accel instances
uint8_t _gyro_instance;
uint8_t _accel_instance;

35
libraries/AP_InertialSensor/AP_InertialSensor_PX4.cpp
View File

@ -7,8 +7,6 @@
const extern AP_HAL::HAL& hal;
#include <DataFlash/DataFlash.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
@ -21,9 +19,7 @@ const extern AP_HAL::HAL& hal;
#include <stdio.h>
AP_InertialSensor_PX4::AP_InertialSensor_PX4(AP_InertialSensor &imu) :
AP_InertialSensor_Backend(imu),
_last_get_sample_timestamp(0),
_last_sample_timestamp(0)
AP_InertialSensor_Backend(imu)
{
}
@ -197,7 +193,7 @@ void AP_InertialSensor_PX4::_new_accel_sample(uint8_t i, accel_report &accel_rep
// apply corrections
_rotate_and_correct_accel(frontend_instance, accel);
_notify_new_accel_raw_sample(frontend_instance, accel);
_notify_new_accel_raw_sample(frontend_instance, accel, accel_report.timestamp);
// save last timestamp
_last_accel_timestamp[i] = accel_report.timestamp;
@ -235,7 +231,7 @@ void AP_InertialSensor_PX4::_new_gyro_sample(uint8_t i, gyro_report &gyro_report
// apply corrections
_rotate_and_correct_gyro(frontend_instance, gyro);
_notify_new_gyro_raw_sample(frontend_instance, gyro);
_notify_new_gyro_raw_sample(frontend_instance, gyro, gyro_report.timestamp);
// save last timestamp
_last_gyro_timestamp[i] = gyro_report.timestamp;
@ -293,7 +289,6 @@ void AP_InertialSensor_PX4::_get_sample()
}
}
}
_last_get_sample_timestamp = hal.scheduler->micros64();
}
bool AP_InertialSensor_PX4::_get_accel_sample(uint8_t i, struct accel_report &accel_report)
@ -302,18 +297,6 @@ 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)),
time_us : hal.scheduler->micros64(),
sample_us : accel_report.timestamp,
AccX : accel_report.x,
AccY : accel_report.y,
AccZ : accel_report.z
};
dataflash->WriteBlock(&pkt, sizeof(pkt));
}
return true;
}
return false;
@ -325,18 +308,6 @@ 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)),
time_us : hal.scheduler->micros64(),
sample_us : gyro_report.timestamp,
GyrX : gyro_report.x,
GyrY : gyro_report.y,
GyrZ : gyro_report.z
};
dataflash->WriteBlock(&pkt, sizeof(pkt));
}
return true;
}
return false;

4
libraries/AP_InertialSensor/AP_InertialSensor_PX4.h
View File

@ -35,12 +35,8 @@ private:
void _get_sample(void);
uint64_t _last_accel_timestamp[INS_MAX_INSTANCES];
uint64_t _last_gyro_timestamp[INS_MAX_INSTANCES];
uint64_t _last_accel_update_timestamp[INS_MAX_INSTANCES];
uint64_t _last_gyro_update_timestamp[INS_MAX_INSTANCES];
float _accel_sample_time[INS_MAX_INSTANCES];
float _gyro_sample_time[INS_MAX_INSTANCES];
uint64_t _last_get_sample_timestamp;
uint64_t _last_sample_timestamp;
void _new_accel_sample(uint8_t i, accel_report &accel_report);
void _new_gyro_sample(uint8_t i, gyro_report &gyro_report);