AP_InertialSensor: allow publishing sample rate to frontend

That information will be used for a "centralized" vibration calculation.
This commit is contained in:
Gustavo Jose de Sousa 2015-08-03 16:22:41 -03:00 committed by Andrew Tridgell
parent ac3a677626
commit 5329e63742
4 changed files with 16 additions and 0 deletions

View File

@ -344,6 +344,7 @@ AP_InertialSensor::AP_InertialSensor() :
#endif
_accel_max_abs_offsets[i] = 3.5f;
_accel_sample_rates[i] = 0;
}
#if INS_VIBRATION_CHECK
for (uint8_t i=0; i<INS_VIBRATION_CHECK_INSTANCES; i++) {

View File

@ -308,6 +308,9 @@ private:
// accelerometer max absolute offsets to be used for calibration
float _accel_max_abs_offsets[INS_MAX_INSTANCES];
// accelerometer sample rate in units of Hz
uint32_t _accel_sample_rates[INS_MAX_INSTANCES];
// temperatures for an instance if available
float _temperature[INS_MAX_INSTANCES];

View File

@ -76,6 +76,12 @@ void AP_InertialSensor_Backend::_set_accel_max_abs_offset(uint8_t instance,
_imu._accel_max_abs_offsets[instance] = max_offset;
}
void AP_InertialSensor_Backend::_set_accel_sample_rate(uint8_t instance,
uint32_t rate)
{
_imu._accel_sample_rates[instance] = rate;
}
// set accelerometer error_count
void AP_InertialSensor_Backend::_set_accel_error_count(uint8_t instance, uint32_t error_count)
{

View File

@ -91,6 +91,12 @@ protected:
// set accelerometer max absolute offset for calibration
void _set_accel_max_abs_offset(uint8_t instance, float offset);
// set accelerometer sample rate
void _set_accel_sample_rate(uint8_t instance, uint32_t rate);
uint32_t _accel_sample_rate(uint8_t instance) const {
return _imu._accel_sample_rates[instance];
}
// publish a temperature value
void _publish_temperature(uint8_t instance, float temperature);