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

sensor_combined: replace accel & gyro integral with value, use float for dt 

Reason: the value is easier to read & handle (for example plotting). In
most places the value is needed, not the integral.

Note that this breaks the replay format for sdlog2 replay
This commit is contained in:
Beat Küng 2016-07-07 10:35:03 +02:00 committed by Lorenz Meier
parent 8e136779ec
commit 9c73eae941
19 changed files with 154 additions and 202 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
msg/ekf2_replay.msg
View File

@ -1,7 +1,7 @@
uint64 time_ref # ekf2 reference time. This is a timestamp passed to the
# estimator which it uses a absolute reference.
uint64 gyro_integral_dt # gyro integration period in us
uint64 accelerometer_integral_dt # accelerometer integration period in us
float32 gyro_integral_dt # gyro integration period in s
float32 accelerometer_integral_dt # accelerometer integration period in s
uint64 magnetometer_timestamp # timestamp of magnetometer measurement in us
uint64 baro_timestamp # timestamp of barometer measurement in us
uint64 rng_timestamp # timestamp of range finder measurement in us
@ -9,8 +9,8 @@ uint64 flow_timestamp # timestamp of optical flow measurement in us
uint64 asp_timestamp # timestamp of airspeed measurement in us
uint64 ev_timestamp # timestamp of external vision measurement in us
float32[3] gyro_integral_rad # integrated gyro vector in rad
float32[3] accelerometer_integral_m_s # integrated accelerometer vector in m/s
float32[3] gyro_rad # gyro vector in rad
float32[3] accelerometer_m_s2 # accelerometer vector in m/s^2
float32[3] magnetometer_ga # magnetometer measurement vector (body fixed NED) in ga
float32 baro_alt_meter # barometer altitude measurement in m

11
msg/sensor_combined.msg
View File

@ -5,15 +5,16 @@
# change with board revisions and sensor updates.
#
int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamp is set to this value, it means the associated sensor values are invalid
int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid
# gyro timstamp is equal to the timestamp of the message
float32[3] gyro_integral_rad # delta angle in the NED body frame in rad in the integration time frame
uint32 gyro_integral_dt # delta time for gyro integral in us
float32[3] gyro_rad # delta angle in the NED body frame in rad
float32 gyro_integral_dt # delta time for gyro integral in s
int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp
float32[3] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2
uint32 accelerometer_integral_dt # delta time for accel integral in us
float32[3] accelerometer_m_s2 # velocity in NED body frame, in m/s^2
float32 accelerometer_integral_dt # delta time for accel integral in s
int32 magnetometer_timestamp_relative # timestamp + magnetometer_timestamp_relative = Magnetometer timestamp
float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss

11
src/drivers/frsky_telemetry/frsky_data.c
View File

@ -204,14 +204,9 @@ void frsky_update_topics()
void frsky_send_frame1(int uart)
{
/* send formatted frame */
float acceleration[3];
float accel_dt = sensor_combined->accelerometer_integral_dt / 1.e6f;
acceleration[0] = sensor_combined->accelerometer_integral_m_s[0] / accel_dt;
acceleration[1] = sensor_combined->accelerometer_integral_m_s[1] / accel_dt;
acceleration[2] = sensor_combined->accelerometer_integral_m_s[2] / accel_dt;
frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(acceleration[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(acceleration[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(acceleration[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_X, roundf(sensor_combined->accelerometer_m_s2[0] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Y, roundf(sensor_combined->accelerometer_m_s2[1] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_ACCEL_Z, roundf(sensor_combined->accelerometer_m_s2[2] * 1000.0f));
frsky_send_data(uart, FRSKY_ID_BARO_ALT_BP,
sensor_combined->baro_alt_meter);

15
src/examples/px4_simple_app/px4_simple_app.c
View File

@ -101,18 +101,15 @@ int px4_simple_app_main(int argc, char *argv[])
struct sensor_combined_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
float sensor_accel[3];
float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
sensor_accel[0] = raw.accelerometer_integral_m_s[0] / accel_dt;
sensor_accel[1] = raw.accelerometer_integral_m_s[1] / accel_dt;
sensor_accel[2] = raw.accelerometer_integral_m_s[2] / accel_dt;
PX4_WARN("[px4_simple_app] Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
(double)sensor_accel[0], (double)sensor_accel[1], (double)sensor_accel[2]);
(double)raw.accelerometer_m_s2[0],
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
/* set att and publish this information for other apps */
att.roll = sensor_accel[0];
att.pitch = sensor_accel[1];
att.yaw = sensor_accel[2];
att.roll = raw.accelerometer_m_s2[0];
att.pitch = raw.accelerometer_m_s2[1];
att.yaw = raw.accelerometer_m_s2[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
}

6
src/lib/terrain_estimation/terrain_estimator.cpp
View File

@ -67,11 +67,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
{
if (attitude->R_valid) {
matrix::Matrix<float, 3, 3> R_att(attitude->R);
matrix::Vector<float, 3> a;
float accel_dt = sensor->accelerometer_integral_dt / 1.e6f;
a(0) = sensor->accelerometer_integral_m_s[0] / accel_dt;
a(1) = sensor->accelerometer_integral_m_s[1] / accel_dt;
a(2) = sensor->accelerometer_integral_m_s[2] / accel_dt;
matrix::Vector<float, 3> a(sensor->accelerometer_m_s2);
matrix::Vector<float, 3> u;
u = R_att * a;
_u_z = u(2) + 9.81f; // compensate for gravity

36
src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
View File

@ -395,19 +395,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_global_position), sub_global_pos, &global_pos);
}
float gyro_rad_s[3];
float gyro_dt = raw.gyro_integral_dt / 1.e6f;
gyro_rad_s[0] = raw.gyro_integral_rad[0] / gyro_dt;
gyro_rad_s[1] = raw.gyro_integral_rad[1] / gyro_dt;
gyro_rad_s[2] = raw.gyro_integral_rad[2] / gyro_dt;
if (!initialized) {
// XXX disabling init for now
initialized = true;
// gyro_offsets[0] += gyro_rad_s[0];
// gyro_offsets[1] += gyro_rad_s[1];
// gyro_offsets[2] += gyro_rad_s[2];
// gyro_offsets[0] += raw.gyro_rad[0];
// gyro_offsets[1] += raw.gyro_rad[1];
// gyro_offsets[2] += raw.gyro_rad[2];
// offset_count++;
// if (hrt_absolute_time() - start_time > 3000000LL) {
@ -433,9 +427,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
sensor_last_timestamp[0] = raw.timestamp;
}
z_k[0] = gyro_rad_s[0] - gyro_offsets[0];
z_k[1] = gyro_rad_s[1] - gyro_offsets[1];
z_k[2] = gyro_rad_s[2] - gyro_offsets[2];
z_k[0] = raw.gyro_rad[0] - gyro_offsets[0];
z_k[1] = raw.gyro_rad[1] - gyro_offsets[1];
z_k[2] = raw.gyro_rad[2] - gyro_offsets[2];
/* update accelerometer measurements */
if (sensor_last_timestamp[1] != raw.timestamp + raw.accelerometer_timestamp_relative) {
@ -476,15 +470,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
last_vel_t = 0;
}
matrix::Vector3f raw_accel;
float accel_dt = raw.accelerometer_integral_dt / 1.e6f;
raw_accel(0) = raw.accelerometer_integral_m_s[0] / accel_dt;
raw_accel(1) = raw.accelerometer_integral_m_s[1] / accel_dt;
raw_accel(2) = raw.accelerometer_integral_m_s[2] / accel_dt;
z_k[3] = raw_accel(0) - acc(0);
z_k[4] = raw_accel(1) - acc(1);
z_k[5] = raw_accel(2) - acc(2);
z_k[3] = raw.accelerometer_m_s2[0] - acc(0);
z_k[4] = raw.accelerometer_m_s2[1] - acc(1);
z_k[5] = raw.accelerometer_m_s2[2] - acc(2);
/* update magnetometer measurements */
if (sensor_last_timestamp[2] != raw.timestamp + raw.magnetometer_timestamp_relative &&
@ -628,9 +616,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
att.pitchacc = x_aposteriori[4];
att.yawacc = x_aposteriori[5];
att.g_comp[0] = raw_accel(0) - acc(0);
att.g_comp[1] = raw_accel(1) - acc(1);
att.g_comp[2] = raw_accel(2) - acc(2);
att.g_comp[0] = raw.accelerometer_m_s2[0] - acc(0);
att.g_comp[1] = raw.accelerometer_m_s2[1] - acc(1);
att.g_comp[2] = raw.accelerometer_m_s2[2] - acc(2);
/* copy offsets */
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));

14
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -328,17 +328,15 @@ void AttitudeEstimatorQ::task_main()
// Feed validator with recent sensor data
if (sensors.timestamp > 0) {
float gyro_dt = sensors.gyro_integral_dt / 1e6;
_gyro(0) = sensors.gyro_integral_rad[0] / gyro_dt;
_gyro(1) = sensors.gyro_integral_rad[1] / gyro_dt;
_gyro(2) = sensors.gyro_integral_rad[2] / gyro_dt;
_gyro(0) = sensors.gyro_rad[0];
_gyro(1) = sensors.gyro_rad[1];
_gyro(2) = sensors.gyro_rad[2];
}
if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
_accel(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
_accel(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
_accel(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
_accel(0) = sensors.accelerometer_m_s2[0];
_accel(1) = sensors.accelerometer_m_s2[1];
_accel(2) = sensors.accelerometer_m_s2[2];
if (_accel.length() < 0.01f) {
warnx("WARNING: degenerate accel!");

2
src/modules/commander/calibration_routines.cpp
View File

@ -273,7 +273,7 @@ enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,
for (unsigned i = 0; i < ndim; i++) {
float di = sensor.accelerometer_integral_m_s[i] / (sensor.accelerometer_integral_dt / 1.e6f);
float di = sensor.accelerometer_m_s2[i];
float d = di - accel_ema[i];
accel_ema[i] += d * w;

44
src/modules/ekf2/ekf2_main.cpp
View File

@ -505,8 +505,16 @@ void Ekf2::task_main()
}
// push imu data into estimator
_ekf.setIMUData(now, sensors.gyro_integral_dt, sensors.accelerometer_integral_dt,
sensors.gyro_integral_rad, sensors.accelerometer_integral_m_s);
float gyro_integral[3];
gyro_integral[0] = sensors.gyro_rad[0] * sensors.gyro_integral_dt;
gyro_integral[1] = sensors.gyro_rad[1] * sensors.gyro_integral_dt;
gyro_integral[2] = sensors.gyro_rad[2] * sensors.gyro_integral_dt;
float accel_integral[3];
accel_integral[0] = sensors.accelerometer_m_s2[0] * sensors.accelerometer_integral_dt;
accel_integral[1] = sensors.accelerometer_m_s2[1] * sensors.accelerometer_integral_dt;
accel_integral[2] = sensors.accelerometer_m_s2[2] * sensors.accelerometer_integral_dt;
_ekf.setIMUData(now, sensors.gyro_integral_dt * 1.e6f, sensors.accelerometer_integral_dt * 1.e6f,
gyro_integral, accel_integral);
// read mag data
if (sensors.magnetometer_timestamp_relative == sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
@ -622,15 +630,14 @@ void Ekf2::task_main()
control_state_s ctrl_state = {};
float gyro_bias[3] = {};
_ekf.get_gyro_bias(gyro_bias);
float gyro_rad_s[3];
float gyro_dt = sensors.gyro_integral_dt / 1.e6f;
gyro_rad_s[0] = sensors.gyro_integral_rad[0] / gyro_dt - gyro_bias[0];
gyro_rad_s[1] = sensors.gyro_integral_rad[1] / gyro_dt - gyro_bias[1];
gyro_rad_s[2] = sensors.gyro_integral_rad[2] / gyro_dt - gyro_bias[2];
ctrl_state.timestamp = hrt_absolute_time();
ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad_s[0]);
ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad_s[1]);
ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad_s[2]);
float gyro_rad[3];
gyro_rad[0] = sensors.gyro_rad[0] - gyro_bias[0];
gyro_rad[1] = sensors.gyro_rad[1] - gyro_bias[1];
gyro_rad[2] = sensors.gyro_rad[2] - gyro_bias[2];
ctrl_state.roll_rate = _lp_roll_rate.apply(gyro_rad[0]);
ctrl_state.pitch_rate = _lp_pitch_rate.apply(gyro_rad[1]);
ctrl_state.yaw_rate = _lp_yaw_rate.apply(gyro_rad[2]);
// Velocity in body frame
float velocity[3];
@ -657,11 +664,7 @@ void Ekf2::task_main()
ctrl_state.q[3] = q(3);
// Acceleration data
matrix::Vector<float, 3> acceleration;
float accel_dt = sensors.accelerometer_integral_dt / 1.e6f;
acceleration(0) = sensors.accelerometer_integral_m_s[0] / accel_dt;
acceleration(1) = sensors.accelerometer_integral_m_s[1] / accel_dt;
acceleration(2) = sensors.accelerometer_integral_m_s[2] / accel_dt;
matrix::Vector<float, 3> acceleration(sensors.accelerometer_m_s2);
float accel_bias[3];
_ekf.get_accel_bias(accel_bias);
@ -722,9 +725,9 @@ void Ekf2::task_main()
att.q[3] = q(3);
att.q_valid = true;
att.rollspeed = gyro_rad_s[0];
att.pitchspeed = gyro_rad_s[1];
att.yawspeed = gyro_rad_s[2];
att.rollspeed = gyro_rad[0];
att.pitchspeed = gyro_rad[1];
att.yawspeed = gyro_rad[2];
// publish vehicle attitude data
if (_att_pub == nullptr) {
@ -917,9 +920,8 @@ void Ekf2::task_main()
replay.accelerometer_integral_dt = sensors.accelerometer_integral_dt;
replay.magnetometer_timestamp = sensors.timestamp + sensors.magnetometer_timestamp_relative;
replay.baro_timestamp = sensors.timestamp + sensors.baro_timestamp_relative;
memcpy(replay.gyro_integral_rad, sensors.gyro_integral_rad, sizeof(replay.gyro_integral_rad));
memcpy(replay.accelerometer_integral_m_s, sensors.accelerometer_integral_m_s,
sizeof(replay.accelerometer_integral_m_s));
memcpy(replay.gyro_rad, sensors.gyro_rad, sizeof(replay.gyro_rad));
memcpy(replay.accelerometer_m_s2, sensors.accelerometer_m_s2, sizeof(replay.accelerometer_m_s2));
memcpy(replay.magnetometer_ga, sensors.magnetometer_ga, sizeof(replay.magnetometer_ga));
replay.baro_alt_meter = sensors.baro_alt_meter;

14
src/modules/ekf2_replay/ekf2_replay_main.cpp
View File

@ -378,15 +378,15 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type)
parseMessage(data, dest_ptr, type);
_sensors.timestamp = replay_part1.time_ref;
_sensors.gyro_integral_dt = replay_part1.gyro_integral_dt;
_sensors.accelerometer_integral_dt = (uint32_t)replay_part1.accelerometer_integral_dt;
_sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt;
_sensors.magnetometer_timestamp_relative = (int32_t)(replay_part1.magnetometer_timestamp - _sensors.timestamp);
_sensors.baro_timestamp_relative = (int32_t)(replay_part1.baro_timestamp - _sensors.timestamp);
_sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad;
_sensors.gyro_integral_rad[1] = replay_part1.gyro_integral_y_rad;
_sensors.gyro_integral_rad[2] = replay_part1.gyro_integral_z_rad;
_sensors.accelerometer_integral_m_s[0] = replay_part1.accelerometer_integral_x_m_s;
_sensors.accelerometer_integral_m_s[1] = replay_part1.accelerometer_integral_y_m_s;
_sensors.accelerometer_integral_m_s[2] = replay_part1.accelerometer_integral_z_m_s;
_sensors.gyro_rad[0] = replay_part1.gyro_x_rad;
_sensors.gyro_rad[1] = replay_part1.gyro_y_rad;
_sensors.gyro_rad[2] = replay_part1.gyro_z_rad;
_sensors.accelerometer_m_s2[0] = replay_part1.accelerometer_x_m_s2;
_sensors.accelerometer_m_s2[1] = replay_part1.accelerometer_y_m_s2;
_sensors.accelerometer_m_s2[2] = replay_part1.accelerometer_z_m_s2;
_sensors.magnetometer_ga[0] = replay_part1.magnetometer_x_ga;
_sensors.magnetometer_ga[1] = replay_part1.magnetometer_y_ga;
_sensors.magnetometer_ga[2] = replay_part1.magnetometer_z_ga;

32
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -1357,23 +1357,23 @@ void AttitudePositionEstimatorEKF::pollData()
/* fill in last data set */
_ekf->dtIMU = deltaT;
_ekf->dAngIMU.x = _sensor_combined.gyro_integral_rad[0];
_ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[1];
_ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[2];
_ekf->angRate.x = _sensor_combined.gyro_rad[0];
_ekf->angRate.y = _sensor_combined.gyro_rad[1];
_ekf->angRate.z = _sensor_combined.gyro_rad[2];
float gyro_dt = _sensor_combined.gyro_integral_dt / 1.e6f;
_ekf->angRate = _ekf->dAngIMU / gyro_dt;
float gyro_dt = _sensor_combined.gyro_integral_dt;
_ekf->dAngIMU = _ekf->angRate * gyro_dt;
perf_count(_perf_gyro);
if (_last_accel != _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative) {
_ekf->dVelIMU.x = _sensor_combined.accelerometer_integral_m_s[0];
_ekf->dVelIMU.y = _sensor_combined.accelerometer_integral_m_s[1];
_ekf->dVelIMU.z = _sensor_combined.accelerometer_integral_m_s[2];
_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
_ekf->accel = _ekf->dVelIMU / accel_dt;
float accel_dt = _sensor_combined.accelerometer_integral_dt;
_ekf->dVelIMU = _ekf->accel * accel_dt;
_last_accel = _sensor_combined.timestamp + _sensor_combined.accelerometer_timestamp_relative;
}
@ -1396,8 +1396,8 @@ void AttitudePositionEstimatorEKF::pollData()
// leave this in as long as larger improvements are still being made.
#if 0
float deltaTIntegral = _sensor_combined.gyro_integral_dt / 1e6f;
float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt / 1e6f;
float deltaTIntegral = _sensor_combined.gyro_integral_dt;
float deltaTIntAcc = _sensor_combined.accelerometer_integral_dt;
static unsigned dtoverflow5 = 0;
static unsigned dtoverflow10 = 0;
@ -1413,10 +1413,10 @@ void AttitudePositionEstimatorEKF::pollData()
(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
(double)(_sensor_combined.accelerometer_integral_m_s[0]),
(double)(_sensor_combined.accelerometer_integral_m_s[1]),
(double)(_sensor_combined.accelerometer_integral_m_s[2]));
(double)(_sensor_combined.gyro_rad[0]), (double)(_sensor_combined.gyro_rad[2]),
(double)(_sensor_combined.accelerometer_m_s2[0]),
(double)(_sensor_combined.accelerometer_m_s2[1]),
(double)(_sensor_combined.accelerometer_m_s2[2]));
PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f",
(double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed);

6
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -1230,11 +1230,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
/* filter speed and altitude for controller */
math::Vector<3> accel_body;
float accel_dt = _sensor_combined.accelerometer_integral_dt / 1.e6f;
accel_body(0) = _sensor_combined.accelerometer_integral_m_s[0] / accel_dt;
accel_body(1) = _sensor_combined.accelerometer_integral_m_s[1] / accel_dt;
accel_body(2) = _sensor_combined.accelerometer_integral_m_s[2] / accel_dt;
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
/* tell TECS to update its state, but let it know when it cannot actually control the plane */

6
src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp
View File

@ -844,11 +844,7 @@ void BlockLocalPositionEstimator::predict()
if (integrate && _sub_att.get().R_valid) {
Matrix3f R_att(_sub_att.get().R);
Vector3f a;
float accel_dt = _sub_sensor.get().accelerometer_integral_dt / 1.e6f;
a(0) = _sub_sensor.get().accelerometer_integral_m_s[0] / accel_dt;
a(1) = _sub_sensor.get().accelerometer_integral_m_s[1] / accel_dt;
a(2) = _sub_sensor.get().accelerometer_integral_m_s[2] / accel_dt;
Vector3f a(_sub_sensor.get().accelerometer_m_s2);
_u = R_att * a;
_u(U_az) += 9.81f; // add g

14
src/modules/mavlink/mavlink_messages.cpp
View File

@ -752,14 +752,12 @@ protected:
mavlink_highres_imu_t msg;
msg.time_usec = sensor.timestamp;
float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
msg.xacc = sensor.accelerometer_integral_m_s[0] / accel_dt;
msg.yacc = sensor.accelerometer_integral_m_s[1] / accel_dt;
msg.zacc = sensor.accelerometer_integral_m_s[2] / accel_dt;
float gyro_dt = sensor.gyro_integral_dt / 1.e6f;
msg.xgyro = sensor.gyro_integral_rad[0] / gyro_dt;
msg.ygyro = sensor.gyro_integral_rad[1] / gyro_dt;
msg.zgyro = sensor.gyro_integral_rad[2] / gyro_dt;
msg.xacc = sensor.accelerometer_m_s2[0];
msg.yacc = sensor.accelerometer_m_s2[1];
msg.zacc = sensor.accelerometer_m_s2[2];
msg.xgyro = sensor.gyro_rad[0];
msg.ygyro = sensor.gyro_rad[1];
msg.zgyro = sensor.gyro_rad[2];
msg.xmag = sensor.magnetometer_ga[0];
msg.ymag = sensor.magnetometer_ga[1];
msg.zmag = sensor.magnetometer_ga[2];

16
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1673,22 +1673,22 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
{
struct sensor_combined_s hil_sensors = {};
hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt;
hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt;
hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt;
hil_sensors.gyro_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]);
hil_sensors.gyro_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]);
hil_sensors.gyro_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]);
_hil_prev_gyro[0] = imu.xgyro;
_hil_prev_gyro[1] = imu.ygyro;
_hil_prev_gyro[2] = imu.zgyro;
hil_sensors.gyro_integral_dt = dt * 1e6f;
hil_sensors.gyro_integral_dt = dt;
hil_sensors.timestamp = timestamp;
hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt;
hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt;
hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt;
hil_sensors.accelerometer_m_s2[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]);
hil_sensors.accelerometer_m_s2[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]);
hil_sensors.accelerometer_m_s2[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]);
_hil_prev_accel[0] = imu.xacc;
_hil_prev_accel[1] = imu.yacc;
_hil_prev_accel[2] = imu.zacc;
hil_sensors.accelerometer_integral_dt = dt * 1e6f;
hil_sensors.accelerometer_integral_dt = dt;
hil_sensors.accelerometer_timestamp_relative = 0;
hil_sensors.magnetometer_ga[0] = imu.xmag;

11
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@ -504,18 +504,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
if (att.R_valid) {
float sensor_accel[3];
float accel_dt = sensor.accelerometer_integral_dt / 1.e6f;
sensor_accel[0] = sensor.accelerometer_integral_m_s[0] / accel_dt - acc_bias[0];
sensor_accel[1] = sensor.accelerometer_integral_m_s[1] / accel_dt - acc_bias[1];
sensor_accel[2] = sensor.accelerometer_integral_m_s[2] / accel_dt - acc_bias[2];
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
sensor.accelerometer_m_s2[1] -= acc_bias[1];
sensor.accelerometer_m_s2[2] -= acc_bias[2];
/* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) {
acc[i] = 0.0f;
for (int j = 0; j < 3; j++) {
acc[i] += PX4_R(att.R, i, j) * sensor_accel[j];
acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
}
}

26
src/modules/sdlog2/sdlog2.c
View File

@ -1551,12 +1551,12 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_RPL1.accelerometer_integral_dt = buf.replay.accelerometer_integral_dt;
log_msg.body.log_RPL1.magnetometer_timestamp = buf.replay.magnetometer_timestamp;
log_msg.body.log_RPL1.baro_timestamp = buf.replay.baro_timestamp;
log_msg.body.log_RPL1.gyro_integral_x_rad = buf.replay.gyro_integral_rad[0];
log_msg.body.log_RPL1.gyro_integral_y_rad = buf.replay.gyro_integral_rad[1];
log_msg.body.log_RPL1.gyro_integral_z_rad = buf.replay.gyro_integral_rad[2];
log_msg.body.log_RPL1.accelerometer_integral_x_m_s = buf.replay.accelerometer_integral_m_s[0];
log_msg.body.log_RPL1.accelerometer_integral_y_m_s = buf.replay.accelerometer_integral_m_s[1];
log_msg.body.log_RPL1.accelerometer_integral_z_m_s = buf.replay.accelerometer_integral_m_s[2];
log_msg.body.log_RPL1.gyro_x_rad = buf.replay.gyro_rad[0];
log_msg.body.log_RPL1.gyro_y_rad = buf.replay.gyro_rad[1];
log_msg.body.log_RPL1.gyro_z_rad = buf.replay.gyro_rad[2];
log_msg.body.log_RPL1.accelerometer_x_m_s2 = buf.replay.accelerometer_m_s2[0];
log_msg.body.log_RPL1.accelerometer_y_m_s2 = buf.replay.accelerometer_m_s2[1];
log_msg.body.log_RPL1.accelerometer_z_m_s2 = buf.replay.accelerometer_m_s2[2];
log_msg.body.log_RPL1.magnetometer_x_ga = buf.replay.magnetometer_ga[0];
log_msg.body.log_RPL1.magnetometer_y_ga = buf.replay.magnetometer_ga[1];
log_msg.body.log_RPL1.magnetometer_z_ga = buf.replay.magnetometer_ga[2];
@ -1660,14 +1660,12 @@ int sdlog2_thread_main(int argc, char *argv[])
if (write_IMU) {
log_msg.msg_type = LOG_IMU_MSG;
float gyro_dt = buf.sensor.gyro_integral_dt / 1.e6f;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_integral_rad[0] / gyro_dt;
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_integral_rad[1] / gyro_dt;
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_integral_rad[2] / gyro_dt;
float accel_dt = buf.sensor.accelerometer_integral_dt / 1.e6f;
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_integral_m_s[0] / accel_dt;
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_integral_m_s[1] / accel_dt;
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_integral_m_s[2] / accel_dt;
log_msg.body.log_IMU.gyro_x = buf.sensor.gyro_rad[0];
log_msg.body.log_IMU.gyro_y = buf.sensor.gyro_rad[1];
log_msg.body.log_IMU.gyro_z = buf.sensor.gyro_rad[2];
log_msg.body.log_IMU.acc_x = buf.sensor.accelerometer_m_s2[0];
log_msg.body.log_IMU.acc_y = buf.sensor.accelerometer_m_s2[1];
log_msg.body.log_IMU.acc_z = buf.sensor.accelerometer_m_s2[2];
log_msg.body.log_IMU.mag_x = buf.sensor.magnetometer_ga[0];
log_msg.body.log_IMU.mag_y = buf.sensor.magnetometer_ga[1];
log_msg.body.log_IMU.mag_z = buf.sensor.magnetometer_ga[2];

16
src/modules/sdlog2/sdlog2_messages.h
View File

@ -520,16 +520,16 @@ struct log_EST5_s {
#define LOG_RPL1_MSG 51
struct log_RPL1_s {
uint64_t time_ref;
uint64_t gyro_integral_dt;
uint64_t accelerometer_integral_dt;
float gyro_integral_dt;
float accelerometer_integral_dt;
uint64_t magnetometer_timestamp;
uint64_t baro_timestamp;
float gyro_integral_x_rad;
float gyro_integral_y_rad;
float gyro_integral_z_rad;
float accelerometer_integral_x_m_s;
float accelerometer_integral_y_m_s;
float accelerometer_integral_z_m_s;
float gyro_x_rad;
float gyro_y_rad;
float gyro_z_rad;
float accelerometer_x_m_s2;
float accelerometer_y_m_s2;
float accelerometer_z_m_s2;
float magnetometer_x_ga;
float magnetometer_y_ga;
float magnetometer_z_ga;

68
src/modules/sensors/sensors.cpp
View File

@ -1074,42 +1074,36 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
}
got_update = true;
math::Vector<3> sensor_value;
if (accel_report.integral_dt != 0) {
math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral);
vect_int = _board_rotation * vect_int;
_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_int(0);
_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_int(1);
_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_int(2);
_last_sensor_data[i].accelerometer_integral_dt = (uint32_t)accel_report.integral_dt;
float dt = accel_report.integral_dt / 1.e6f;
sensor_value = vect_int / dt;
_last_sensor_data[i].accelerometer_integral_dt = dt;
_last_sensor_data[i].accelerometer_m_s2[0] = vect_int(0) / dt;
_last_sensor_data[i].accelerometer_m_s2[1] = vect_int(1) / dt;
_last_sensor_data[i].accelerometer_m_s2[2] = vect_int(2) / dt;
} else {
//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
//using the value instead of the integral (the integral is the prefered choice)
math::Vector<3> vect_val(accel_report.x, accel_report.y, accel_report.z);
vect_val = _board_rotation * vect_val;
sensor_value = vect_val;
if (_last_accel_timestamp[i] == 0) {
_last_accel_timestamp[i] = accel_report.timestamp - 1000;
}
_last_sensor_data[i].accelerometer_integral_dt =
(uint32_t)(accel_report.timestamp - _last_accel_timestamp[i]);
float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f;
_last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt;
_last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt;
_last_sensor_data[i].accelerometer_integral_m_s[2] = vect_val(2) * dt;
(accel_report.timestamp - _last_accel_timestamp[i]) / 1.e6f;
_last_sensor_data[i].accelerometer_m_s2[0] = vect_val(0);
_last_sensor_data[i].accelerometer_m_s2[1] = vect_val(1);
_last_sensor_data[i].accelerometer_m_s2[2] = vect_val(2);
}
_last_accel_timestamp[i] = accel_report.timestamp;
_accel.voter.put(i, accel_report.timestamp, sensor_value.data,
_accel.voter.put(i, accel_report.timestamp, _last_sensor_data[i].accelerometer_m_s2,
accel_report.error_count, _accel.priority[i]);
}
}
@ -1119,9 +1113,9 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
_accel.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
raw.accelerometer_integral_m_s[0] = _last_sensor_data[best_index].accelerometer_integral_m_s[0];
raw.accelerometer_integral_m_s[1] = _last_sensor_data[best_index].accelerometer_integral_m_s[1];
raw.accelerometer_integral_m_s[2] = _last_sensor_data[best_index].accelerometer_integral_m_s[2];
raw.accelerometer_m_s2[0] = _last_sensor_data[best_index].accelerometer_m_s2[0];
raw.accelerometer_m_s2[1] = _last_sensor_data[best_index].accelerometer_m_s2[1];
raw.accelerometer_m_s2[2] = _last_sensor_data[best_index].accelerometer_m_s2[2];
raw.accelerometer_integral_dt = _last_sensor_data[best_index].accelerometer_integral_dt;
_accel.last_best_vote = (uint8_t)best_index;
}
@ -1147,42 +1141,36 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
}
got_update = true;
math::Vector<3> sensor_value;
if (gyro_report.integral_dt != 0) {
math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral);
vect_int = _board_rotation * vect_int;
_last_sensor_data[i].gyro_integral_rad[0] = vect_int(0);
_last_sensor_data[i].gyro_integral_rad[1] = vect_int(1);
_last_sensor_data[i].gyro_integral_rad[2] = vect_int(2);
_last_sensor_data[i].gyro_integral_dt = (uint32_t)gyro_report.integral_dt;
float dt = gyro_report.integral_dt / 1.e6f;
sensor_value = vect_int / dt;
_last_sensor_data[i].gyro_integral_dt = dt;
_last_sensor_data[i].gyro_rad[0] = vect_int(0) / dt;
_last_sensor_data[i].gyro_rad[1] = vect_int(1) / dt;
_last_sensor_data[i].gyro_rad[2] = vect_int(2) / dt;
} else {
//this is undesirable: a driver did not set the integral, so we have to construct it ourselves
//using the value instead of the integral (the integral is the prefered choice)
math::Vector<3> vect_val(gyro_report.x, gyro_report.y, gyro_report.z);
vect_val = _board_rotation * vect_val;
sensor_value = vect_val;
if (_last_sensor_data[i].timestamp == 0) {
_last_sensor_data[i].timestamp = gyro_report.timestamp - 1000;
}
_last_sensor_data[i].gyro_integral_dt =
(uint32_t)(gyro_report.timestamp - _last_sensor_data[i].timestamp);
float dt = _last_sensor_data[i].gyro_integral_dt / 1.e6f;
_last_sensor_data[i].gyro_integral_rad[0] = vect_val(0) * dt;
_last_sensor_data[i].gyro_integral_rad[1] = vect_val(1) * dt;
_last_sensor_data[i].gyro_integral_rad[2] = vect_val(2) * dt;
(gyro_report.timestamp - _last_sensor_data[i].timestamp) / 1.e6f;
_last_sensor_data[i].gyro_rad[0] = vect_val(0);
_last_sensor_data[i].gyro_rad[1] = vect_val(1);
_last_sensor_data[i].gyro_rad[2] = vect_val(2);
}
_last_sensor_data[i].timestamp = gyro_report.timestamp;
_gyro.voter.put(i, gyro_report.timestamp, sensor_value.data,
_gyro.voter.put(i, gyro_report.timestamp, _last_sensor_data[i].gyro_rad,
gyro_report.error_count, _gyro.priority[i]);
}
}
@ -1192,9 +1180,9 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
_gyro.voter.get_best(hrt_absolute_time(), &best_index);
if (best_index >= 0) {
raw.gyro_integral_rad[0] = _last_sensor_data[best_index].gyro_integral_rad[0];
raw.gyro_integral_rad[1] = _last_sensor_data[best_index].gyro_integral_rad[1];
raw.gyro_integral_rad[2] = _last_sensor_data[best_index].gyro_integral_rad[2];
raw.gyro_rad[0] = _last_sensor_data[best_index].gyro_rad[0];
raw.gyro_rad[1] = _last_sensor_data[best_index].gyro_rad[1];
raw.gyro_rad[2] = _last_sensor_data[best_index].gyro_rad[2];
raw.gyro_integral_dt = _last_sensor_data[best_index].gyro_integral_dt;
raw.timestamp = _last_sensor_data[best_index].timestamp;
_gyro.last_best_vote = (uint8_t)best_index;