Skip to content

Commit

Permalink
GCS_Mavlink: Adapt to upstream mavlink changes to the SCALED_PRESSURE…
Browse files Browse the repository at this point in the history
… message
  • Loading branch information
amilcarlucas authored and tridge committed Jul 28, 2020
1 parent 3435575 commit 49499d1
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 4 deletions.
2 changes: 1 addition & 1 deletion libraries/GCS_MAVLink/GCS.h
Original file line number Diff line number Diff line change
Expand Up @@ -236,7 +236,7 @@ class GCS_MAVLINK
void send_rc_channels_raw() const;
void send_raw_imu();

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_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, int16_t temperature_press_diff));
void send_scaled_pressure();
void send_scaled_pressure2();
virtual void send_scaled_pressure3(); // allow sub to override this
Expand Down
8 changes: 5 additions & 3 deletions libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1716,14 +1716,15 @@ void GCS_MAVLINK::send_scaled_imu(uint8_t instance, void (*send_fn)(mavlink_chan
// send data for barometer and airspeed sensors instances. In the
// case that we run out of instances of one before the other we send
// the relevant fields as 0.
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_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, int16_t temperature_press_diff))
{
const AP_Baro &barometer = AP::baro();

bool have_data = false;

float press_abs = 0.0f;
float temperature = 0.0f;
float temperature = 0.0f; // Absolute pressure temperature
float temperature_press_diff = 0.0f; // TODO: Differential pressure temperature
if (instance < barometer.num_instances()) {
press_abs = barometer.get_pressure(instance) * 0.01f;
temperature = barometer.get_temperature(instance)*100;
Expand All @@ -1747,7 +1748,8 @@ void GCS_MAVLINK::send_scaled_pressure_instance(uint8_t instance, void (*send_fn
AP_HAL::millis(),
press_abs, // hectopascal
press_diff, // hectopascal
temperature); // 0.01 degrees C
temperature, // 0.01 degrees C
temperature_press_diff); // 0.01 degrees C
}

void GCS_MAVLINK::send_scaled_pressure()
Expand Down

0 comments on commit 49499d1

Please sign in to comment.