Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Extend use of AP_BATTERY_ENABLED #26106

Merged
merged 12 commits into from
Feb 6, 2024
7 changes: 7 additions & 0 deletions libraries/AP_BattMonitor/AP_BattMonitor.cpp
Original file line number Diff line number Diff line change
@@ -1,4 +1,9 @@
#include "AP_BattMonitor_config.h"

#if AP_BATTERY_ENABLED

#include "AP_BattMonitor.h"

#include "AP_BattMonitor_Analog.h"
#include "AP_BattMonitor_SMBus.h"
#include "AP_BattMonitor_SMBus_Solo.h"
Expand Down Expand Up @@ -1117,3 +1122,5 @@ AP_BattMonitor &battery()
}

};

#endif // AP_BATTERY_ENABLED
7 changes: 6 additions & 1 deletion libraries/AP_BattMonitor/AP_BattMonitor.h
Original file line number Diff line number Diff line change
@@ -1,12 +1,15 @@
#pragma once

#include "AP_BattMonitor_config.h"

#if AP_BATTERY_ENABLED

#include <AP_Common/AP_Common.h>
#include <AP_Param/AP_Param.h>
#include <AP_Math/AP_Math.h>
#include <AP_TemperatureSensor/AP_TemperatureSensor_config.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include "AP_BattMonitor_Params.h"
#include "AP_BattMonitor_config.h"

// maximum number of battery monitors
#ifndef AP_BATT_MONITOR_MAX_INSTANCES
Expand Down Expand Up @@ -315,3 +318,5 @@ class AP_BattMonitor
namespace AP {
AP_BattMonitor &battery();
};

#endif // AP_BATTERY_ENABLED
6 changes: 6 additions & 0 deletions libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,10 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "AP_BattMonitor_config.h"

#if AP_BATTERY_ENABLED

#include <AP_Common/AP_Common.h>
#include <AP_HAL/AP_HAL.h>
#include "AP_BattMonitor.h"
Expand Down Expand Up @@ -334,3 +338,5 @@ void AP_BattMonitor_Backend::update_consumed(AP_BattMonitor::BattMonitor_State &
state.consumed_wh += 0.001 * mah * state.voltage;
}
}

#endif // AP_BATTERY_ENABLED
9 changes: 8 additions & 1 deletion libraries/AP_BattMonitor/AP_BattMonitor_Backend.h
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,14 @@
*/
#pragma once

#include <AP_Common/AP_Common.h>
#include "AP_BattMonitor_config.h"

#if AP_BATTERY_ENABLED

#include "AP_BattMonitor.h"

#include <AP_Common/AP_Common.h>

class AP_BattMonitor_Backend
{
public:
Expand Down Expand Up @@ -141,3 +146,5 @@ struct BattMonitorScript_State {
float temperature=nanf(""); // Battery temperature in degrees Celsius
};
#endif // AP_BATTERY_SCRIPTING_ENABLED

#endif // AP_BATTERY_ENABLED
5 changes: 3 additions & 2 deletions libraries/AP_BattMonitor/AP_BattMonitor_Logging.cpp
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "AP_BattMonitor_config.h"
#include <AP_Logger/AP_Logger_config.h>

#if HAL_LOGGING_ENABLED
#if AP_BATTERY_ENABLED && HAL_LOGGING_ENABLED

#include "AP_BattMonitor_Backend.h"
#include <AP_Logger/AP_Logger.h>
Expand Down Expand Up @@ -88,4 +89,4 @@ void AP_BattMonitor_Backend::Log_Write_BCL(const uint8_t instance, const uint64_
#endif
}

#endif // HAL_LOGGING_ENABLED
#endif // AP_BATTERY_ENABLED && HAL_LOGGING_ENABLED
6 changes: 6 additions & 0 deletions libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp
Original file line number Diff line number Diff line change
@@ -1,3 +1,7 @@
#include "AP_BattMonitor_config.h"

#if AP_BATTERY_ENABLED

#include <AP_Common/AP_Common.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include "AP_BattMonitor_Params.h"
Expand Down Expand Up @@ -172,3 +176,5 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
AP_BattMonitor_Params::AP_BattMonitor_Params(void) {
AP_Param::setup_object_defaults(this, var_info);
}

#endif // AP_BATTERY_ENABLED
3 changes: 0 additions & 3 deletions libraries/AP_HAL_ChibiOS/hwdef/HEEWING-F405/hwdef.dat
Original file line number Diff line number Diff line change
Expand Up @@ -160,9 +160,6 @@ define HAL_BATT_CURR_SCALE 26.7
define HAL_WITH_DSP FALSE
define HAL_GYROFFT_ENABLED 0

# --------------------- save flash ----------------------
define AP_BATTMON_SMBUS_ENABLE 0

include ../include/minimize_fpv_osd.inc
include ../include/save_some_flash.inc

Expand Down
8 changes: 0 additions & 8 deletions libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat
Original file line number Diff line number Diff line change
Expand Up @@ -171,14 +171,6 @@ define STM32_PWM_USE_ADVANCED TRUE

define HAL_WITH_DSP FALSE

# save some flash

# disable SMBUS and fuel battery monitors to save flash
define AP_BATTMON_SMBUS_ENABLE 0
define AP_BATTMON_FUEL_ENABLE 0
define AP_BATTMON_SYNTHETIC_CURRENT_ENABLED 0
define HAL_BATTMON_INA2XX_ENABLED 0

# disable parachute and sprayer to save flash
define HAL_PARACHUTE_ENABLED 0
define HAL_SPRAYER_ENABLED 0
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_Hott_Telem/AP_Hott_Telem.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -109,6 +109,7 @@ void AP_Hott_Telem::send_EAM(void)
uint8_t stop_byte = 0x7D; //#44 stop
} msg {};

#if AP_BATTERY_ENABLED
const AP_BattMonitor &battery = AP::battery();
if (battery.num_instances() > 0) {
msg.batt1_voltage = uint16_t(battery.voltage(0) * 10);
Expand All @@ -125,6 +126,7 @@ void AP_Hott_Telem::send_EAM(void)
if (battery.consumed_mah(used_mah)) {
msg.batt_used = used_mah * 0.1;
}
#endif // AP_BATTERY_ENABLED

const AP_Baro &baro = AP::baro();
msg.temp1 = uint8_t(baro.get_temperature(0) + 20.5);
Expand Down
5 changes: 5 additions & 0 deletions libraries/AP_LTM_Telem/AP_LTM_Telem.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,7 @@ void AP_LTM_Telem::send_Gframe(void)
// Sensors frame
void AP_LTM_Telem::send_Sframe(void)
{
#if AP_BATTERY_ENABLED
const AP_BattMonitor &battery = AP::battery();
const uint16_t volt = (uint16_t) roundf(battery.voltage() * 1000.0f); // battery voltage (expects value in mV)
float current;
Expand All @@ -124,6 +125,10 @@ void AP_LTM_Telem::send_Sframe(void)
}
// note: max. current value we can send is 65.536 A
const uint16_t amp = (uint16_t) roundf(current * 100.0f); // current sensor (expects value in hundredth of A)
#else
const uint16_t volt = 0;
const uint16_t amp = 0;
#endif

// airspeed in m/s if available and enabled - even if not used - otherwise send 0
uint8_t airspeed = 0; // airspeed sensor (m/s)
Expand Down
35 changes: 35 additions & 0 deletions libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,9 @@ void AP_MSP_Telem_Backend::setup_wfq_scheduler(void)
set_scheduler_entry(ATTITUDE, 200, 200); // 5Hz attitude
set_scheduler_entry(ALTITUDE, 250, 250); // 4Hz altitude(cm) and velocity(cm/s)
set_scheduler_entry(ANALOG, 250, 250); // 4Hz rssi + batt
#if AP_BATTERY_ENABLED
set_scheduler_entry(BATTERY_STATE, 500, 500); // 2Hz battery
#endif
#if HAL_WITH_ESC_TELEM
set_scheduler_entry(ESC_SENSOR_DATA, 500, 500); // 2Hz ESC telemetry
#endif
Expand Down Expand Up @@ -114,7 +116,9 @@ bool AP_MSP_Telem_Backend::is_packet_ready(uint8_t idx, bool queue_empty)
case ATTITUDE: // Attitude
case ALTITUDE: // Altitude and Vario
case ANALOG: // Rssi, Battery, mAh, Current
#if AP_BATTERY_ENABLED
case BATTERY_STATE: // voltage, capacity, current, mAh
#endif
#if HAL_WITH_ESC_TELEM
case ESC_SENSOR_DATA: // esc temp + rpm
#endif
Expand Down Expand Up @@ -155,10 +159,12 @@ void AP_MSP_Telem_Backend::process_packet(uint8_t idx)
_msp_port.c_state = MSP_IDLE;
}

#if AP_BATTERY_ENABLED
uint8_t AP_MSP_Telem_Backend::calc_cell_count(const float battery_voltage)
{
return floorf((battery_voltage / CELLFULL) + 1);
}
#endif

float AP_MSP_Telem_Backend::get_vspeed_ms(void) const
{
Expand Down Expand Up @@ -215,6 +221,7 @@ void AP_MSP_Telem_Backend::update_gps_state(gps_state_t &gps_state)
}
}

#if AP_BATTERY_ENABLED
void AP_MSP_Telem_Backend::update_battery_state(battery_state_t &battery_state)
{
memset(&battery_state, 0, sizeof(battery_state));
Expand All @@ -241,6 +248,7 @@ void AP_MSP_Telem_Backend::update_battery_state(battery_state_t &battery_state)
battery_state.batt_cellcount = cc;
}
}
#endif // AP_BATTERY_ENABLED

void AP_MSP_Telem_Backend::update_airspeed(airspeed_state_t &airspeed_state)
{
Expand Down Expand Up @@ -334,7 +342,9 @@ void AP_MSP_Telem_Backend::enable_warnings()
return;
}
BIT_SET(msp->_osd_config.enabled_warnings, OSD_WARNING_FAIL_SAFE);
#if AP_BATTERY_ENABLED
BIT_SET(msp->_osd_config.enabled_warnings, OSD_WARNING_BATTERY_CRITICAL);
#endif
}

void AP_MSP_Telem_Backend::process_incoming_data()
Expand Down Expand Up @@ -466,8 +476,10 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_command(uint16_t cmd_msp,
return msp_process_out_altitude(dst);
case MSP_ANALOG:
return msp_process_out_analog(dst);
#if AP_BATTERY_ENABLED
case MSP_BATTERY_STATE:
return msp_process_out_battery_state(dst);
#endif
case MSP_UID:
return msp_process_out_uid(dst);
#if HAL_WITH_ESC_TELEM
Expand Down Expand Up @@ -905,6 +917,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_altitude(sbuf_t *dst)

MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_analog(sbuf_t *dst)
{
#if AP_BATTERY_ENABLED
battery_state_t battery_state;
update_battery_state(battery_state);

Expand All @@ -922,10 +935,27 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_analog(sbuf_t *dst)
current_ca : (int16_t)constrain_int32(battery_state.batt_current_a * 100, -0x8000, 0x7FFF), // current A to cA (0.01 steps, range is -320A to 320A)
voltage_cv : (uint16_t)constrain_int32(battery_state.batt_voltage_v * 100,0,0xFFFF) // battery voltage in 0.01V steps
};
#else
float rssi;
const struct PACKED {
uint8_t voltage_dv;
uint16_t mah;
uint16_t rssi;
int16_t current_ca;
uint16_t voltage_cv;
} analog {
voltage_dv : 0,
mah : 0,
rssi : uint16_t(get_rssi(rssi) ? constrain_float(rssi,0,1) * 1023 : 0), // rssi 0-1 to 0-1023)
current_ca : 0,
voltage_cv : 0
};
#endif
sbuf_write_data(dst, &analog, sizeof(analog));
return MSP_RESULT_ACK;
}

#if AP_BATTERY_ENABLED
MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst)
{
const AP_MSP *msp = AP::msp();
Expand Down Expand Up @@ -956,6 +986,7 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_battery_state(sbuf_t *dst
sbuf_write_data(dst, &battery, sizeof(battery));
return MSP_RESULT_ACK;
}
#endif

MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_esc_sensor_data(sbuf_t *dst)
{
Expand Down Expand Up @@ -1104,7 +1135,9 @@ void AP_MSP_Telem_Backend::hide_osd_items(void)
if (msp == nullptr) {
return;
}
#if AP_BATTERY_ENABLED
const AP_Notify &notify = AP::notify();
#endif
// clear all and only set the flashing ones
BIT_CLEAR(osd_hidden_items_bitmask, OSD_GPS_SATS);
BIT_CLEAR(osd_hidden_items_bitmask, OSD_HOME_DIR);
Expand Down Expand Up @@ -1146,11 +1179,13 @@ void AP_MSP_Telem_Backend::hide_osd_items(void)
if (msp->_msp_status.flight_mode_focus) {
BIT_SET(osd_hidden_items_bitmask, OSD_CRAFT_NAME);
}
#if AP_BATTERY_ENABLED
// flash battery on failsafe
if (notify.flags.failsafe_battery) {
BIT_SET(osd_hidden_items_bitmask, OSD_AVG_CELL_VOLTAGE);
BIT_SET(osd_hidden_items_bitmask, OSD_MAIN_BATT_VOLTAGE);
}
#endif
// flash rtc if no time available
#if AP_RTC_ENABLED
uint64_t time_usec;
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_Motors/AP_Motors6DOF.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -359,6 +359,7 @@ void AP_Motors6DOF::output_armed_stabilizing()
}
}

#if AP_BATTERY_ENABLED
const AP_BattMonitor &battery = AP::battery();

// Current limiting
Expand All @@ -384,6 +385,7 @@ void AP_Motors6DOF::output_armed_stabilizing()
batt_current_ratio = predicted_current_ratio;
}
_output_limited += (_dt / (_dt + _batt_current_time_constant)) * (1 - batt_current_ratio);
#endif

_output_limited = constrain_float(_output_limited, 0.0f, 1.0f);

Expand Down
4 changes: 4 additions & 0 deletions libraries/AP_Motors/AP_MotorsMulticopter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -345,6 +345,7 @@ void AP_MotorsMulticopter::update_throttle_filter()
// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
float AP_MotorsMulticopter::get_current_limit_max_throttle()
{
#if AP_BATTERY_ENABLED
AP_BattMonitor &battery = AP::battery();

const uint8_t batt_idx = thr_lin.get_battery_index();
Expand Down Expand Up @@ -376,6 +377,9 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()

// limit max throttle
return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit);
#else
return 1.0;
#endif
}

#if HAL_LOGGING_ENABLED
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,7 @@ float Thrust_Linearization::remove_thrust_curve_and_volt_scaling(float throttle)
// update_lift_max from battery voltage - used for voltage compensation
void Thrust_Linearization::update_lift_max_from_batt_voltage()
{
#if AP_BATTERY_ENABLED
// sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately
float _batt_voltage = motors.has_option(AP_Motors::MotorOptions::BATT_RAW_VOLTAGE) ? AP::battery().voltage(batt_idx) : AP::battery().voltage_resting_estimate(batt_idx);
Expand All @@ -177,6 +178,7 @@ void Thrust_Linearization::update_lift_max_from_batt_voltage()
// calculate lift max
float thrust_curve_expo = constrain_float(curve_expo, -1.0, 1.0);
lift_max = batt_voltage_filt.get() * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt.get() * batt_voltage_filt.get();
#endif
}

// return gain scheduling gain based on voltage and air density
Expand Down
6 changes: 5 additions & 1 deletion libraries/AP_Notify/Display.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -406,7 +406,9 @@ void Display::update_all()
{
update_text(0);
update_mode(1);
#if AP_BATTERY_ENABLED
update_battery(2);
#endif
#if AP_GPS_ENABLED
update_gps(3);
#endif
Expand Down Expand Up @@ -530,6 +532,7 @@ void Display::update_ekf(uint8_t r)
}
}

#if AP_BATTERY_ENABLED
void Display::update_battery(uint8_t r)
{
char msg [DISPLAY_MESSAGE_SIZE];
Expand All @@ -541,7 +544,8 @@ void Display::update_battery(uint8_t r)
snprintf(msg, DISPLAY_MESSAGE_SIZE, "BAT:%4.2fV --%% ", (double)battery.voltage()) ;
}
draw_text(COLUMN(0), ROW(r), msg);
}
}
#endif // AP_BATTERY_ENABLED

void Display::update_mode(uint8_t r)
{
Expand Down
2 changes: 2 additions & 0 deletions libraries/AP_OSD/AP_OSD.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -446,6 +446,7 @@ void AP_OSD::update_stats()
// maximum altitude
alt = -alt;
_stats.max_alt_m = fmaxf(_stats.max_alt_m, alt);
#if AP_BATTERY_ENABLED
// maximum current
AP_BattMonitor &battery = AP::battery();
float amps;
Expand All @@ -457,6 +458,7 @@ void AP_OSD::update_stats()
if (voltage > 0) {
_stats.min_voltage_v = fminf(_stats.min_voltage_v, voltage);
}
#endif
#if AP_RSSI_ENABLED
// minimum rssi
AP_RSSI *ap_rssi = AP_RSSI::get_singleton();
Expand Down
Loading
Loading