dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Update mag interface

This commit is contained in:
kamilritz 2020-01-21 14:49:19 +01:00 committed by Paul Riseborough
parent 856961ba85
commit 9f0d8ed59e
3 changed files with 10 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
EKF/estimator_interface.cpp
View File

@ -130,7 +130,7 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
imu_sample_new.delta_ang = Vector3f(delta_ang);
imu_sample_new.delta_vel = Vector3f(delta_vel);
// convert time from us to secs
void EstimatorInterface::setMagData(const magSample &mag_sample)
imu_sample_new.delta_ang_dt = delta_ang_dt / 1e6f;
imu_sample_new.delta_vel_dt = delta_vel_dt / 1e6f;
imu_sample_new.time_us = time_usec;
@ -138,7 +138,6 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u
setIMUData(imu_sample_new);
}
void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
{
if (!_initialised || _mag_buffer_fail) {
return;
@ -158,11 +157,12 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
// downsample to highest possible sensor rate
// by taking the average of incoming sample
_mag_sample_count++;
_mag_data_sum += Vector3f(data);
_mag_timestamp_sum += time_usec / 1000; // Dividing by 1000 to avoid overflow
_mag_data_sum += mag_sample.mag;
_mag_timestamp_sum += mag_sample.time_us / 1000; // Dividing by 1000 to avoid overflow
// limit data rate to prevent data being lost
if ((time_usec - _time_last_mag) > _min_obs_interval_us) {
if ((mag_sample.time_us - _time_last_mag) > _min_obs_interval_us) {
_time_last_mag = mag_sample.time_us;
magSample mag_sample_new;
@ -175,7 +175,6 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float (&data)[3])
_mag_buffer.push(mag_sample_new);
_time_last_mag = time_usec;
_mag_sample_count = 0;
_mag_data_sum.setZero();
_mag_timestamp_sum = 0;

2
EKF/estimator_interface.h
View File

@ -177,7 +177,7 @@ public:
// legacy interface for compatibility (2018-09-14)
void setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, uint64_t delta_vel_dt, float (&delta_ang)[3], float (&delta_vel)[3]);
void setMagData(uint64_t time_usec, float (&data)[3]);
void setMagData(const magSample &mag_sample);
void setGpsData(const gps_message &gps);

7
test/sensor_simulator/mag.cpp
View File

@ -15,9 +15,10 @@ Mag::~Mag()
void Mag::send(uint64_t time)
{
float mag[3];
_mag_data.copyTo(mag);
_ekf->setMagData(time,mag);
magSample mag_sample;
mag_sample.mag = _mag_data;
mag_sample.time_us = time;
_ekf->setMagData(mag_sample);
}
void Mag::setData(const Vector3f& mag)