diff --git a/libraries/GCS_MAVLink/GCS.cpp b/libraries/GCS_MAVLink/GCS.cpp index 7a887f2db7..84a29b6934 100644 --- a/libraries/GCS_MAVLink/GCS.cpp +++ b/libraries/GCS_MAVLink/GCS.cpp @@ -195,7 +195,7 @@ void GCS::update_sensor_status_flags() } #endif -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED const AP_BattMonitor &battery = AP::battery(); control_sensors_present |= MAV_SYS_STATUS_SENSOR_BATTERY; if (battery.num_instances() > 0) { diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index c430aa302e..77be39ba6d 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -82,7 +82,8 @@ #include #endif -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#include +#if AP_BATTERY_ENABLED #include #endif #include @@ -221,7 +222,7 @@ void GCS_MAVLINK::send_mcu_status(void) // returns the battery remaining percentage if valid, -1 otherwise int8_t GCS_MAVLINK::battery_remaining_pct(const uint8_t instance) const { -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED uint8_t percentage; return AP::battery().capacity_remaining_pct(percentage, instance) ? MIN(percentage, INT8_MAX) : -1; #else @@ -231,7 +232,7 @@ int8_t GCS_MAVLINK::battery_remaining_pct(const uint8_t instance) const { void GCS_MAVLINK::send_battery_status(const uint8_t instance) const { -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED // catch the battery backend not supporting the required number of cells static_assert(sizeof(AP_BattMonitor::cells) >= (sizeof(uint16_t) * MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN), "Not enough battery cells for the MAVLink message"); @@ -357,7 +358,7 @@ void GCS_MAVLINK::send_battery_status(const uint8_t instance) const // returns true if all battery instances were reported bool GCS_MAVLINK::send_battery_status() { -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED const AP_BattMonitor &battery = AP::battery(); for(uint8_t i = 0; i < AP_BATT_MONITOR_MAX_INSTANCES; i++) { @@ -2493,7 +2494,7 @@ void GCS::setup_uarts() // report battery2 state void GCS_MAVLINK::send_battery2() { -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED const AP_BattMonitor &battery = AP::battery(); if (battery.num_instances() > 1) { @@ -4385,7 +4386,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_do_set_mission_current(const mavlink_comm MAV_RESULT GCS_MAVLINK::handle_command_battery_reset(const mavlink_command_long_t &packet) { -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED const uint16_t battery_mask = packet.param1; const float percentage = packet.param2; if (AP::battery().reset_remaining_mask(battery_mask, percentage)) { @@ -5273,7 +5274,7 @@ void GCS_MAVLINK::send_sys_status() if (!gcs().vehicle_initialised()) { return; } -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED const AP_BattMonitor &battery = AP::battery(); float battery_current; const int8_t battery_remaining = battery_remaining_pct(AP_BATT_PRIMARY_INSTANCE); @@ -5302,7 +5303,7 @@ void GCS_MAVLINK::send_sys_status() control_sensors_enabled, control_sensors_health, static_cast(AP::scheduler().load_average() * 1000), -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED battery.gcs_voltage() * 1000, // mV battery_current, // in 10mA units battery_remaining, // in % @@ -6567,7 +6568,7 @@ void GCS_MAVLINK::send_high_latency2() const AP_AHRS &ahrs = AP::ahrs(); Location global_position_current; UNUSED_RESULT(ahrs.get_location(global_position_current)); -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED const int8_t battery_remaining = battery_remaining_pct(AP_BATT_PRIMARY_INSTANCE); #endif @@ -6630,7 +6631,7 @@ void GCS_MAVLINK::send_high_latency2() const 0, // [dm] Maximum error vertical position since last message high_latency_air_temperature(), // [degC] Air temperature from airspeed sensor 0, // [dm/s] Maximum climb rate magnitude since last message -#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY) +#if AP_BATTERY_ENABLED battery_remaining, // [%] Battery level (-1 if field not provided). #else -1,