fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-17 01:07:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Stream scaled IMU for Snapdragon Flight using VISLAM

Browse Source 
This is temporary (and for Snapdragon Flight + VISLAM only) until there is a proper solution to get unfiltered IMU data for VIOs etc.
This commit is contained in:
ChristophTobler
2018-01-05 11:41:26 +01:00
committed by Lorenz Meier
parent 545f8c4452
commit 3ffc1fd25b
2 changed files with 108 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+90
View File
@@ -96,6 +96,8 @@
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/mount_orientation.h>
#include <uORB/topics/collision_report.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/uORB.h>
@@ -726,6 +728,93 @@ protected:
};
// TEMP This is temporary for the Snapdragon Flight and VISLAM to get unfiltered IMU data
class MavlinkStreamScaledIMU : public MavlinkStream
{
public:
const char *get_name() const
{
return MavlinkStreamScaledIMU::get_name_static();
}
static const char *get_name_static()
{
return "SCALED_IMU";
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_SCALED_IMU;
}
uint16_t get_id()
{
return get_id_static();
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamScaledIMU(mavlink);
}
unsigned get_size()
{
return _raw_accel_sub->is_published() ? (MAVLINK_MSG_ID_SCALED_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
}
private:
MavlinkOrbSubscription *_raw_accel_sub;
MavlinkOrbSubscription *_raw_gyro_sub;
uint64_t _raw_accel_time;
uint64_t _raw_gyro_time;
struct sensor_gyro_s _sensor_gyro;
// do not allow top copy this class
MavlinkStreamScaledIMU(MavlinkStreamScaledIMU &);
MavlinkStreamScaledIMU &operator = (const MavlinkStreamScaledIMU &);
protected:
explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink),
_raw_accel_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_accel))),
_raw_gyro_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_gyro))),
_raw_accel_time(0),
_raw_gyro_time(0),
_sensor_gyro{}
{}
bool send(const hrt_abstime t)
{
struct sensor_accel_s sensor_accel = {};
bool raw_accel_updated = _raw_accel_sub->update(&_raw_accel_time, &sensor_accel);
_raw_gyro_sub->update(&_raw_gyro_time, &_sensor_gyro);
// send if raw_accel has been updated and use the newest gyro values we have
if (raw_accel_updated) {
mavlink_scaled_imu_t msg = {};
msg.time_boot_ms = sensor_accel.timestamp / 1000;
msg.xacc = (int16_t)(sensor_accel.x_raw / CONSTANTS_ONE_G); // [milli g]
msg.yacc = (int16_t)(sensor_accel.y_raw / CONSTANTS_ONE_G); // [milli g]
msg.zacc = (int16_t)(sensor_accel.z_raw / CONSTANTS_ONE_G); // [milli g]
msg.xgyro = _sensor_gyro.x_raw; // [milli rad/s]
msg.ygyro = _sensor_gyro.y_raw; // [milli rad/s]
msg.zgyro = _sensor_gyro.z_raw; // [milli rad/s]
msg.xmag = 0;
msg.ymag = 0;
msg.zmag = 0;
mavlink_msg_scaled_imu_send_struct(_mavlink->get_channel(), &msg);
return true;
}
return false;
}
};
class MavlinkStreamAttitude : public MavlinkStream
{
public:
@@ -4290,6 +4379,7 @@ const StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamCommandLong::new_instance, &MavlinkStreamCommandLong::get_name_static, &MavlinkStreamCommandLong::get_id_static),
new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static, &MavlinkStreamSysStatus::get_id_static),
new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static, &MavlinkStreamHighresIMU::get_id_static),
new StreamListItem(&MavlinkStreamScaledIMU::new_instance, &MavlinkStreamScaledIMU::get_name_static, &MavlinkStreamScaledIMU::get_id_static),
new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static, &MavlinkStreamAttitude::get_id_static),
new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static, &MavlinkStreamAttitudeQuaternion::get_id_static),
new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static, &MavlinkStreamVFRHUD::get_id_static),
src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp
+18 -14
View File
@@ -599,11 +599,6 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
// ACCEL
// write raw data (without rotation)
accel_report.x_raw = data.accel_m_s2_x;
accel_report.y_raw = data.accel_m_s2_y;
accel_report.z_raw = data.accel_m_s2_z;
float xraw_f = data.accel_m_s2_x;
float yraw_f = data.accel_m_s2_y;
float zraw_f = data.accel_m_s2_z;
@@ -611,6 +606,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// MPU9250 driver from DriverFramework does not provide any raw values
// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
accel_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000];
accel_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000];
accel_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000];
// adjust values according to the calibration
float x_in_new = (xraw_f - _accel_calibration.x_offset) * _accel_calibration.x_scale;
float y_in_new = (yraw_f - _accel_calibration.y_offset) * _accel_calibration.y_scale;
@@ -630,11 +631,6 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
// GYRO
// write raw data (withoud rotation)
gyro_report.x_raw = data.gyro_rad_s_x;
gyro_report.y_raw = data.gyro_rad_s_y;
gyro_report.z_raw = data.gyro_rad_s_z;
xraw_f = data.gyro_rad_s_x;
yraw_f = data.gyro_rad_s_y;
zraw_f = data.gyro_rad_s_z;
@@ -642,6 +638,12 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// MPU9250 driver from DriverFramework does not provide any raw values
// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
gyro_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000];
gyro_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000];
gyro_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000];
// adjust values according to the calibration
float x_gyro_in_new = (xraw_f - _gyro_calibration.x_offset) * _gyro_calibration.x_scale;
float y_gyro_in_new = (yraw_f - _gyro_calibration.y_offset) * _gyro_calibration.y_scale;
@@ -706,16 +708,18 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
mag_report.range_ga = -1.0f;
mag_report.device_id = m_id.dev_id;
mag_report.x_raw = 0;
mag_report.y_raw = 0;
mag_report.z_raw = 0;
xraw_f = data.mag_ga_x;
yraw_f = data.mag_ga_y;
zraw_f = data.mag_ga_z;
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
// MPU9250 driver from DriverFramework does not provide any raw values
// TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM
mag_report.x_raw = xraw_f * 1000; // (int16) [Gs * 1000]
mag_report.y_raw = yraw_f * 1000; // (int16) [Gs * 1000]
mag_report.z_raw = zraw_f * 1000; // (int16) [Gs * 1000]
mag_report.x = (xraw_f - _mag_calibration.x_offset) * _mag_calibration.x_scale;
mag_report.y = (yraw_f - _mag_calibration.y_offset) * _mag_calibration.y_scale;
mag_report.z = (zraw_f - _mag_calibration.z_offset) * _mag_calibration.z_scale;