diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bb44ef4a1b..5a1fc7a6eb 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -96,6 +96,8 @@ #include #include #include +#include +#include #include @@ -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), diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp index 2c4d371081..111e725fe2 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp @@ -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;