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

mavlink: SCALED_IMU streams add temperature

Browse Source 
- preference for accel or gyro temperature before mag
This commit is contained in:
Daniel Agar
2022-02-22 16:25:00 -05:00
parent 5b5d428189
commit 8de2c80b34
3 changed files with 90 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/streams/SCALED_IMU.hpp
+30 -1
View File
@@ -39,6 +39,7 @@
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
class MavlinkStreamScaledIMU : public MavlinkStream
{
@@ -64,6 +65,7 @@ private:
explicit MavlinkStreamScaledIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 0};
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), 0};
uORB::Subscription _sensor_mag_sub{ORB_ID(sensor_mag), 0};
bool send() override
@@ -71,6 +73,8 @@ private:
if (_vehicle_imu_sub.updated() || _sensor_mag_sub.updated()) {
mavlink_scaled_imu_t msg{};
float temperature = NAN;
vehicle_imu_s imu;
if (_vehicle_imu_sub.copy(&imu)) {
@@ -89,6 +93,18 @@ private:
msg.xgyro = gyro(0);
msg.ygyro = gyro(1);
msg.zgyro = gyro(2);
// IMU temperature
vehicle_imu_status_s vehicle_imu_status;
if (_vehicle_imu_status_sub.copy(&vehicle_imu_status)) {
if (PX4_ISFINITE(vehicle_imu_status.temperature_accel)) {
temperature = vehicle_imu_status.temperature_accel;
} else if (PX4_ISFINITE(vehicle_imu_status.temperature_gyro)) {
temperature = vehicle_imu_status.temperature_gyro;
}
}
}
sensor_mag_s sensor_mag;
@@ -101,7 +117,20 @@ private:
msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
msg.temperature = sensor_mag.temperature;
if (!PX4_ISFINITE(temperature) && PX4_ISFINITE(sensor_mag.temperature)) {
temperature = sensor_mag.temperature;
}
}
if (PX4_ISFINITE(temperature)) {
// degrees -> centi-degrees constrained to int16
msg.temperature = math::constrain(roundf(temperature * 100.f), (float)INT16_MIN, (float)INT16_MAX);
if (msg.temperature == 0) {
// if the IMU is at 0C it must send 1 (0.01C).
msg.temperature = 1;
}
}
mavlink_msg_scaled_imu_send_struct(_mavlink->get_channel(), &msg);
src/modules/mavlink/streams/SCALED_IMU2.hpp
+30 -1
View File
@@ -39,6 +39,7 @@
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
class MavlinkStreamScaledIMU2 : public MavlinkStream
{
@@ -64,6 +65,7 @@ private:
explicit MavlinkStreamScaledIMU2(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 1};
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), 1};
uORB::Subscription _sensor_mag_sub{ORB_ID(sensor_mag), 1};
bool send() override
@@ -71,6 +73,8 @@ private:
if (_vehicle_imu_sub.updated() || _sensor_mag_sub.updated()) {
mavlink_scaled_imu2_t msg{};
float temperature = NAN;
vehicle_imu_s imu;
if (_vehicle_imu_sub.copy(&imu)) {
@@ -89,6 +93,18 @@ private:
msg.xgyro = gyro(0);
msg.ygyro = gyro(1);
msg.zgyro = gyro(2);
// IMU temperature
vehicle_imu_status_s vehicle_imu_status;
if (_vehicle_imu_status_sub.copy(&vehicle_imu_status)) {
if (PX4_ISFINITE(vehicle_imu_status.temperature_accel)) {
temperature = vehicle_imu_status.temperature_accel;
} else if (PX4_ISFINITE(vehicle_imu_status.temperature_gyro)) {
temperature = vehicle_imu_status.temperature_gyro;
}
}
}
sensor_mag_s sensor_mag;
@@ -101,7 +117,20 @@ private:
msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
msg.temperature = sensor_mag.temperature;
if (!PX4_ISFINITE(temperature) && PX4_ISFINITE(sensor_mag.temperature)) {
temperature = sensor_mag.temperature;
}
}
if (PX4_ISFINITE(temperature)) {
// degrees -> centi-degrees constrained to int16
msg.temperature = math::constrain(roundf(temperature * 100.f), (float)INT16_MIN, (float)INT16_MAX);
if (msg.temperature == 0) {
// if the IMU is at 0C it must send 1 (0.01C).
msg.temperature = 1;
}
}
mavlink_msg_scaled_imu2_send_struct(_mavlink->get_channel(), &msg);
src/modules/mavlink/streams/SCALED_IMU3.hpp
+30 -1
View File
@@ -39,6 +39,7 @@
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_imu_status.h>
class MavlinkStreamScaledIMU3 : public MavlinkStream
{
@@ -64,6 +65,7 @@ private:
explicit MavlinkStreamScaledIMU3(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _vehicle_imu_sub{ORB_ID(vehicle_imu), 2};
uORB::Subscription _vehicle_imu_status_sub{ORB_ID(vehicle_imu_status), 2};
uORB::Subscription _sensor_mag_sub{ORB_ID(sensor_mag), 2};
bool send() override
@@ -71,6 +73,8 @@ private:
if (_vehicle_imu_sub.updated() || _sensor_mag_sub.updated()) {
mavlink_scaled_imu3_t msg{};
float temperature = NAN;
vehicle_imu_s imu;
if (_vehicle_imu_sub.copy(&imu)) {
@@ -89,6 +93,18 @@ private:
msg.xgyro = gyro(0);
msg.ygyro = gyro(1);
msg.zgyro = gyro(2);
// IMU temperature
vehicle_imu_status_s vehicle_imu_status;
if (_vehicle_imu_status_sub.copy(&vehicle_imu_status)) {
if (PX4_ISFINITE(vehicle_imu_status.temperature_accel)) {
temperature = vehicle_imu_status.temperature_accel;
} else if (PX4_ISFINITE(vehicle_imu_status.temperature_gyro)) {
temperature = vehicle_imu_status.temperature_gyro;
}
}
}
sensor_mag_s sensor_mag;
@@ -101,7 +117,20 @@ private:
msg.xmag = sensor_mag.x * 1000.0f; // Gauss -> MilliGauss
msg.ymag = sensor_mag.y * 1000.0f; // Gauss -> MilliGauss
msg.zmag = sensor_mag.z * 1000.0f; // Gauss -> MilliGauss
msg.temperature = sensor_mag.temperature;
if (!PX4_ISFINITE(temperature) && PX4_ISFINITE(sensor_mag.temperature)) {
temperature = sensor_mag.temperature;
}
}
if (PX4_ISFINITE(temperature)) {
// degrees -> centi-degrees constrained to int16
msg.temperature = math::constrain(roundf(temperature * 100.f), (float)INT16_MIN, (float)INT16_MAX);
if (msg.temperature == 0) {
// if the IMU is at 0C it must send 1 (0.01C).
msg.temperature = 1;
}
}
mavlink_msg_scaled_imu3_send_struct(_mavlink->get_channel(), &msg);