Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: split MSG_SCALED_PRESSURE into three separate messages

Browse Source
This commit is contained in:
Peter Barker 2018-12-07 17:30:32 +11:00 committed by Peter Barker
parent e498883624
commit cc5ad393e7
2 changed files with 37 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/GCS_MAVLink/GCS.h
View File

@ -53,6 +53,8 @@ enum ap_message : uint8_t {
MSG_SCALED_IMU2, MSG_SCALED_IMU2,
MSG_SCALED_IMU3, MSG_SCALED_IMU3,
MSG_SCALED_PRESSURE, MSG_SCALED_PRESSURE,
MSG_SCALED_PRESSURE2,
MSG_SCALED_PRESSURE3,
MSG_SENSOR_OFFSETS, MSG_SENSOR_OFFSETS,
MSG_GPS_RAW, MSG_GPS_RAW,
MSG_GPS_RTK, MSG_GPS_RTK,
@ -186,8 +188,11 @@ public:
void send_system_time(); void send_system_time();
void send_radio_in(); void send_radio_in();
void send_raw_imu(); void send_raw_imu();
virtual void send_scaled_pressure3(); // allow sub to override this
void send_scaled_pressure_instance(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature));
void send_scaled_pressure(); void send_scaled_pressure();
void send_scaled_pressure2();
virtual void send_scaled_pressure3(); // allow sub to override this
void send_sensor_offsets(); void send_sensor_offsets();
virtual void send_simstate() const; virtual void send_simstate() const;
void send_ahrs(); void send_ahrs();

67
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -872,6 +872,8 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
{ MAVLINK_MSG_ID_SCALED_IMU2, MSG_SCALED_IMU2}, { MAVLINK_MSG_ID_SCALED_IMU2, MSG_SCALED_IMU2},
{ MAVLINK_MSG_ID_SCALED_IMU3, MSG_SCALED_IMU3}, { MAVLINK_MSG_ID_SCALED_IMU3, MSG_SCALED_IMU3},
{ MAVLINK_MSG_ID_SCALED_PRESSURE, MSG_SCALED_PRESSURE}, { MAVLINK_MSG_ID_SCALED_PRESSURE, MSG_SCALED_PRESSURE},
{ MAVLINK_MSG_ID_SCALED_PRESSURE2, MSG_SCALED_PRESSURE2},
{ MAVLINK_MSG_ID_SCALED_PRESSURE3, MSG_SCALED_PRESSURE3},
{ MAVLINK_MSG_ID_SENSOR_OFFSETS, MSG_SENSOR_OFFSETS}, { MAVLINK_MSG_ID_SENSOR_OFFSETS, MSG_SENSOR_OFFSETS},
{ MAVLINK_MSG_ID_GPS_RAW_INT, MSG_GPS_RAW}, { MAVLINK_MSG_ID_GPS_RAW_INT, MSG_GPS_RAW},
{ MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK}, { MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK},
@ -1695,58 +1697,41 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan
} }
// sub overrides this to send on-board temperature void GCS_MAVLINK::send_scaled_pressure_instance(uint8_t instance, void (*send_fn)(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature))
void GCS_MAVLINK::send_scaled_pressure3()
{ {
const AP_Baro &barometer = AP::baro(); const AP_Baro &barometer = AP::baro();
if (barometer.num_instances() < 3) { if (instance >= barometer.num_instances()) {
return;
}
if (!HAVE_PAYLOAD_SPACE(chan, SCALED_PRESSURE3)) {
return; return;
} }
const float pressure = barometer.get_pressure(2);
mavlink_msg_scaled_pressure3_send(
chan,
AP_HAL::millis(),
pressure*0.01f, // hectopascal
(pressure - barometer.get_ground_pressure(2))*0.01f, // hectopascal
barometer.get_temperature(2)*100); // 0.01 degrees C
}
void GCS_MAVLINK::send_scaled_pressure()
{
uint32_t now = AP_HAL::millis();
const AP_Baro &barometer = AP::baro();
float pressure = barometer.get_pressure(0);
float diff_pressure = 0; // pascal float diff_pressure = 0; // pascal
AP_Airspeed *airspeed = AP_Airspeed::get_singleton(); AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
if (airspeed != nullptr) { if (airspeed != nullptr) {
diff_pressure = airspeed->get_differential_pressure(); diff_pressure = airspeed->get_differential_pressure();
} }
mavlink_msg_scaled_pressure_send( send_fn(
chan, chan,
now, AP_HAL::millis(),
pressure*0.01f, // hectopascal barometer.get_pressure(instance) * 0.01f, // hectopascal
diff_pressure*0.01f, // hectopascal diff_pressure*0.01f, // hectopascal
barometer.get_temperature(0)*100); // 0.01 degrees C barometer.get_temperature(instance)*100); // 0.01 degrees C
}
if (barometer.num_instances() > 1 && void GCS_MAVLINK::send_scaled_pressure()
HAVE_PAYLOAD_SPACE(chan, SCALED_PRESSURE2)) { {
pressure = barometer.get_pressure(1); send_scaled_pressure_instance(0, mavlink_msg_scaled_pressure_send);
mavlink_msg_scaled_pressure2_send( }
chan,
now,
pressure*0.01f, // hectopascal
(pressure - barometer.get_ground_pressure(1))*0.01f, // hectopascal
barometer.get_temperature(1)*100); // 0.01 degrees C
}
send_scaled_pressure3(); void GCS_MAVLINK::send_scaled_pressure2()
{
send_scaled_pressure_instance(1, mavlink_msg_scaled_pressure2_send);
}
void GCS_MAVLINK::send_scaled_pressure3()
{
send_scaled_pressure_instance(2, mavlink_msg_scaled_pressure3_send);
} }
void GCS_MAVLINK::send_sensor_offsets() void GCS_MAVLINK::send_sensor_offsets()
@ -3790,6 +3775,16 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
send_scaled_pressure(); send_scaled_pressure();
break; break;
case MSG_SCALED_PRESSURE2:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE2);
send_scaled_pressure2();
break;
case MSG_SCALED_PRESSURE3:
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE3);
send_scaled_pressure3();
break;
case MSG_SENSOR_OFFSETS: case MSG_SENSOR_OFFSETS:
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
send_sensor_offsets(); send_sensor_offsets();