zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

sensors move to matrix lib

sbg
Daniel Agar 7 years ago committed by Beat Küng
parent
ad2b96b523
commit
3a0a896a9c
4 changed files with 30 additions and 32 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 6
      src/modules/sensors/temperature_compensation.cpp
  2. 11
      src/modules/sensors/temperature_compensation.h
  3. 41
      src/modules/sensors/voted_sensors_update.cpp
  4. 4
      src/modules/sensors/voted_sensors_update.h

6
src/modules/sensors/temperature_compensation.cpp
Unescape View File

@ -377,8 +377,8 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc
return -1; return -1;
} }
int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Vector<3> &sensor_data, float temperature, int TemperatureCompensation::apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data,
float *offsets, float *scales) float temperature, float *offsets, float *scales)
{ {
if (_parameters.gyro_tc_enable != 1) { if (_parameters.gyro_tc_enable != 1) {
return 0; return 0;
@ -406,7 +406,7 @@ int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Ve
return 1; return 1;
} }
int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::Vector<3> &sensor_data, int TemperatureCompensation::apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data,
float temperature, float *offsets, float *scales) float temperature, float *offsets, float *scales)
{ {
if (_parameters.accel_tc_enable != 1) { if (_parameters.accel_tc_enable != 1) {

11
src/modules/sensors/temperature_compensation.h
Unescape View File

@ -89,14 +89,13 @@ public:
* 1: corrections applied but no changes to offsets & scales, * 1: corrections applied but no changes to offsets & scales,
* 2: corrections applied and offsets & scales updated * 2: corrections applied and offsets & scales updated
*/ */
int apply_corrections_gyro(int topic_instance, math::Vector<3> &sensor_data, float temperature, int apply_corrections_gyro(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets,
float *offsets, float *scales); float *scales);
int apply_corrections_accel(int topic_instance, math::Vector<3> &sensor_data, float temperature, int apply_corrections_accel(int topic_instance, matrix::Vector3f &sensor_data, float temperature, float *offsets,
float *offsets, float *scales); float *scales);
int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature, int apply_corrections_baro(int topic_instance, float &sensor_data, float temperature, float *offsets, float *scales);
float *offsets, float *scales);
/** output current configuration status to console */ /** output current configuration status to console */
void print_status(); void print_status();

41
src/modules/sensors/voted_sensors_update.cpp
Unescape View File

@ -134,14 +134,13 @@ void VotedSensorsUpdate::deinit()
void VotedSensorsUpdate::parameters_update() void VotedSensorsUpdate::parameters_update()
{ {
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
/* fine tune board offset */ /* fine tune board offset */
math::Matrix<3, 3> board_rotation_offset; matrix::Dcmf board_rotation_offset = matrix::Eulerf(
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[0],
M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[1],
M_DEG_TO_RAD_F * _parameters.board_offset[2]); M_DEG_TO_RAD_F * _parameters.board_offset[2]);
_board_rotation = board_rotation_offset * _board_rotation; _board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation);
// initialze all mag rotations with the board rotation in case there is no calibration data available // initialze all mag rotations with the board rotation in case there is no calibration data available
for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX; ++topic_instance) { for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX; ++topic_instance) {
@ -513,7 +512,7 @@ void VotedSensorsUpdate::parameters_update()
/* now get the mag rotation */ /* now get the mag rotation */
if (mag_rot >= 0) { if (mag_rot >= 0) {
// Set external magnetometers to use the parameter value // Set external magnetometers to use the parameter value
get_rot_matrix((enum Rotation)mag_rot, &_mag_rotation[topic_instance]); _mag_rotation[topic_instance] = get_rot_matrix((enum Rotation)mag_rot);
} else { } else {
// Set internal magnetometers to use the board rotation // Set internal magnetometers to use the board rotation
@ -567,7 +566,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
_accel_device_id[uorb_index] = accel_report.device_id; _accel_device_id[uorb_index] = accel_report.device_id;
math::Vector<3> accel_data; matrix::Vector3f accel_data;
if (accel_report.integral_dt != 0) { if (accel_report.integral_dt != 0) {
/* /*
@ -579,8 +578,9 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
// convert the delta velocities to an equivalent acceleration before application of corrections // convert the delta velocities to an equivalent acceleration before application of corrections
float dt_inv = 1.e6f / accel_report.integral_dt; float dt_inv = 1.e6f / accel_report.integral_dt;
accel_data = math::Vector<3>(accel_report.x_integral * dt_inv, accel_report.y_integral * dt_inv, accel_data = matrix::Vector3f(accel_report.x_integral * dt_inv,
accel_report.z_integral * dt_inv); accel_report.y_integral * dt_inv,
accel_report.z_integral * dt_inv);
_last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt; _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt;
@ -589,7 +589,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw)
// Correct each sensor for temperature effects // Correct each sensor for temperature effects
// Filtering and/or downsampling of temperature should be performed in the driver layer // Filtering and/or downsampling of temperature should be performed in the driver layer
accel_data = math::Vector<3>(accel_report.x, accel_report.y, accel_report.z); accel_data = matrix::Vector3f(accel_report.x, accel_report.y, accel_report.z);
// handle the cse where this is our first output // handle the cse where this is our first output
if (_last_accel_timestamp[uorb_index] == 0) { if (_last_accel_timestamp[uorb_index] == 0) {
@ -674,7 +674,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
_gyro_device_id[uorb_index] = gyro_report.device_id; _gyro_device_id[uorb_index] = gyro_report.device_id;
math::Vector<3> gyro_rate; matrix::Vector3f gyro_rate;
if (gyro_report.integral_dt != 0) { if (gyro_report.integral_dt != 0) {
/* /*
@ -686,8 +686,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
// convert the delta angles to an equivalent angular rate before application of corrections // convert the delta angles to an equivalent angular rate before application of corrections
float dt_inv = 1.e6f / gyro_report.integral_dt; float dt_inv = 1.e6f / gyro_report.integral_dt;
gyro_rate = math::Vector<3>(gyro_report.x_integral * dt_inv, gyro_report.y_integral * dt_inv, gyro_rate = matrix::Vector3f(gyro_report.x_integral * dt_inv,
gyro_report.z_integral * dt_inv); gyro_report.y_integral * dt_inv,
gyro_report.z_integral * dt_inv);
_last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt; _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt;
@ -696,7 +697,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw)
// Correct each sensor for temperature effects // Correct each sensor for temperature effects
// Filtering and/or downsampling of temperature should be performed in the driver layer // Filtering and/or downsampling of temperature should be performed in the driver layer
gyro_rate = math::Vector<3>(gyro_report.x, gyro_report.y, gyro_report.z); gyro_rate = matrix::Vector3f(gyro_report.x, gyro_report.y, gyro_report.z);
// handle the case where this is our first output // handle the case where this is our first output
if (_last_sensor_data[uorb_index].timestamp == 0) { if (_last_sensor_data[uorb_index].timestamp == 0) {
@ -782,7 +783,7 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
_mag.priority[uorb_index] = (uint8_t)priority; _mag.priority[uorb_index] = (uint8_t)priority;
} }
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); matrix::Vector3f vect(mag_report.x, mag_report.y, mag_report.z);
vect = _mag_rotation[uorb_index] * vect; vect = _mag_rotation[uorb_index] * vect;
_last_sensor_data[uorb_index].magnetometer_ga[0] = vect(0); _last_sensor_data[uorb_index].magnetometer_ga[0] = vect(0);
@ -790,8 +791,7 @@ void VotedSensorsUpdate::mag_poll(struct sensor_combined_s &raw)
_last_sensor_data[uorb_index].magnetometer_ga[2] = vect(2); _last_sensor_data[uorb_index].magnetometer_ga[2] = vect(2);
_last_mag_timestamp[uorb_index] = mag_report.timestamp; _last_mag_timestamp[uorb_index] = mag_report.timestamp;
_mag.voter.put(uorb_index, mag_report.timestamp, vect.data, _mag.voter.put(uorb_index, mag_report.timestamp, vect.data(), mag_report.error_count, _mag.priority[uorb_index]);
mag_report.error_count, _mag.priority[uorb_index]);
} }
} }
@ -851,15 +851,14 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw)
_baro_device_id[uorb_index] = baro_report.device_id; _baro_device_id[uorb_index] = baro_report.device_id;
got_update = true; got_update = true;
math::Vector<3> vect(baro_report.altitude, 0.f, 0.f); matrix::Vector3f vect(baro_report.altitude, 0.f, 0.f);
_last_sensor_data[uorb_index].baro_alt_meter = baro_report.altitude; _last_sensor_data[uorb_index].baro_alt_meter = baro_report.altitude;
_last_sensor_data[uorb_index].baro_temp_celcius = baro_report.temperature; _last_sensor_data[uorb_index].baro_temp_celcius = baro_report.temperature;
_last_baro_pressure[uorb_index] = corrected_pressure; _last_baro_pressure[uorb_index] = corrected_pressure;
_last_baro_timestamp[uorb_index] = baro_report.timestamp; _last_baro_timestamp[uorb_index] = baro_report.timestamp;
_baro.voter.put(uorb_index, baro_report.timestamp, vect.data, _baro.voter.put(uorb_index, baro_report.timestamp, vect.data(), baro_report.error_count, _baro.priority[uorb_index]);
baro_report.error_count, _baro.priority[uorb_index]);
} }
} }

4
src/modules/sensors/voted_sensors_update.h
Unescape View File

@ -253,8 +253,8 @@ private:
uint64_t _last_mag_timestamp[MAG_COUNT_MAX]; /**< latest full timestamp */ uint64_t _last_mag_timestamp[MAG_COUNT_MAX]; /**< latest full timestamp */
uint64_t _last_baro_timestamp[BARO_COUNT_MAX]; /**< latest full timestamp */ uint64_t _last_baro_timestamp[BARO_COUNT_MAX]; /**< latest full timestamp */
math::Matrix<3, 3> _board_rotation = {}; /**< rotation matrix for the orientation that the board is mounted */ matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
math::Matrix<3, 3> _mag_rotation[MAG_COUNT_MAX] = {}; /**< rotation matrix for the orientation that the external mag0 is mounted */ matrix::Dcmf _mag_rotation[MAG_COUNT_MAX]; /**< rotation matrix for the orientation that the external mag0 is mounted */
const Parameters &_parameters; const Parameters &_parameters;
const bool _hil_enabled; /**< is hardware-in-the-loop mode enabled? */ const bool _hil_enabled; /**< is hardware-in-the-loop mode enabled? */

Write Preview
Loading…
Cancel
Save