Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-27 02:04:00 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: integrate BattMonitor

Browse Source
This commit is contained in:
Randy Mackay 2013-09-29 21:54:39 +09:00
parent 8b06a12fa4
commit c64d444b5a
10 changed files with 49 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
ArduPlane/ArduPlane.pde
View File

@ -72,6 +72,7 @@
#include <AP_TECS.h>
#include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
// Pre-AP_HAL compatibility
#include "compat.h"
@ -299,9 +300,6 @@ static AP_HAL::AnalogSource *rssi_analog_source;
static AP_HAL::AnalogSource *vcc_pin;
static AP_HAL::AnalogSource * batt_volt_pin;
static AP_HAL::AnalogSource * batt_curr_pin;
////////////////////////////////////////////////////////////////////////////////
// Relay
////////////////////////////////////////////////////////////////////////////////
@ -376,6 +374,9 @@ static struct {
// has the saved mode for failsafe been set?
uint8_t saved_mode_set:1;
// flag to hold whether battery low voltage threshold has been breached
uint8_t low_battery:1;
// saved flight mode
enum FlightMode saved_mode;
@ -465,20 +466,7 @@ static int32_t altitude_error_cm;
////////////////////////////////////////////////////////////////////////////////
// Battery Sensors
////////////////////////////////////////////////////////////////////////////////
static struct {
// Battery pack 1 voltage. Initialized above the low voltage
// threshold to pre-load the filter and prevent low voltage events
// at startup.
float voltage;
// Battery pack 1 instantaneous currrent draw. Amperes
float current_amps;
// Totalized current (Amp-hours) from battery 1
float current_total_mah;
// true when a low battery event has happened
bool low_batttery;
// time when current was last read
uint32_t last_time_ms;
} battery;
AP_BattMonitor battery;
////////////////////////////////////////////////////////////////////////////////
// Airspeed Sensors
@ -744,13 +732,12 @@ void setup() {
notify.init();
battery.init();
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
vcc_pin = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
batt_volt_pin = hal.analogin->channel(g.battery_volt_pin);
batt_curr_pin = hal.analogin->channel(g.battery_curr_pin);
init_ardupilot();
// initialise the main loop scheduler

12
ArduPlane/GCS_Mavlink.pde
View File

@ -216,14 +216,12 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
uint16_t battery_current = -1;
uint8_t battery_remaining = -1;
if (battery.last_time_ms != 0) {
if (g.pack_capacity != 0) {
battery_remaining = (100.0 * (g.pack_capacity - battery.current_total_mah) / g.pack_capacity);
}
battery_current = battery.current_amps * 100;
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
battery_remaining = battery.capacity_remaining_pct();
battery_current = battery.current_amps() * 100;
}
if (g.battery_monitoring == 3) {
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) {
/*setting a out-of-range value.
* It informs to external devices that
* it cannot be calculated properly just by voltage*/
@ -236,7 +234,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
control_sensors_enabled,
control_sensors_health,
(uint16_t)(scheduler.load_average(20000) * 1000),
battery.voltage * 1000, // mV
battery.voltage() * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,

6
ArduPlane/Log.pde
View File

@ -383,10 +383,10 @@ static void Log_Write_Current()
struct log_Current pkt = {
LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
throttle_in : channel_throttle->control_in,
battery_voltage : (int16_t)(battery.voltage * 100.0),
current_amps : (int16_t)(battery.current_amps * 100.0),
battery_voltage : (int16_t)(battery.voltage() * 100.0),
current_amps : (int16_t)(battery.current_amps() * 100.0),
board_voltage : board_voltage(),
current_total : battery.current_total_mah
current_total : battery.current_total_mah()
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}

22
ArduPlane/Parameters.h
View File

@ -124,11 +124,11 @@ public:
k_param_compass_enabled,
k_param_compass,
k_param_battery_monitoring,
k_param_volt_div_ratio,
k_param_curr_amp_per_volt,
k_param_battery_monitoring, // unused
k_param_volt_div_ratio, // unused
k_param_curr_amp_per_volt, // unused
k_param_input_voltage, // deprecated, can be deleted
k_param_pack_capacity,
k_param_pack_capacity, // unused
k_param_sonar_enabled,
k_param_ahrs, // AHRS group
k_param_barometer, // barometer ground calibration
@ -160,9 +160,10 @@ public:
//
// Battery monitoring parameters
//
k_param_rssi_pin = 167,
k_param_battery_volt_pin,
k_param_battery_curr_pin, // 169
k_param_battery = 166,
k_param_rssi_pin,
k_param_battery_volt_pin, // unused
k_param_battery_curr_pin, // unused - 169
//
// 170: Radio settings
@ -373,18 +374,11 @@ public:
#endif
AP_Int8 compass_enabled;
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
AP_Int8 flap_1_percent;
AP_Int8 flap_1_speed;
AP_Int8 flap_2_percent;
AP_Int8 flap_2_speed;
AP_Float volt_div_ratio;
AP_Float curr_amp_per_volt;
AP_Float curr_amp_offset;
AP_Int32 pack_capacity; // Battery pack capacity less reserve
AP_Int8 rssi_pin;
AP_Int8 battery_volt_pin;
AP_Int8 battery_curr_pin;
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
AP_Int8 stick_mixing;
AP_Float takeoff_throttle_min_speed;

52
ArduPlane/Parameters.pde
View File

@ -677,54 +677,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Advanced
GSCALAR(flap_2_speed, "FLAP_2_SPEED", FLAP_2_SPEED),
// @Param: BATT_MONITOR
// @DisplayName: Battery monitoring
// @Description: Controls enabling monitoring of the battery's voltage and current
// @Values: 0:Disabled,3:Voltage Only,4:Voltage and Current
// @User: Standard
GSCALAR(battery_monitoring, "BATT_MONITOR", BATTERY_MONITORING),
// @Param: VOLT_DIVIDER
// @DisplayName: Voltage Divider
// @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_DIVIDER). For the 3DR Power brick on APM2 or Pixhawk, this should be set to 10.1. For the PX4 using the PX4IO power supply this should be set to 1.
// @User: Advanced
GSCALAR(volt_div_ratio, "VOLT_DIVIDER", VOLT_DIV_RATIO),
// @Param: APM_PER_VOLT
// @DisplayName: Apms per volt
// @Description: Number of amps that a 1V reading on the current sensor corresponds to. On the APM2 or Pixhawk using the 3DR Power brick this should be set to 17.
// @Units: A/V
// @User: Standard
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT),
// @Param: AMP_OFFSET
// @DisplayName: AMP offset
// @Description: Voltage offset at zero current on current sensor
// @Units: Volts
// @User: Standard
GSCALAR(curr_amp_offset, "AMP_OFFSET", 0),
// @Param: BATT_CAPACITY
// @DisplayName: Battery capacity
// @Description: Capacity of the battery in mAh when full
// @Units: mAh
// @User: Standard
GSCALAR(pack_capacity, "BATT_CAPACITY", 1760),
// @Param: BATT_VOLT_PIN
// @DisplayName: Battery Voltage sensing pin
// @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 13. On the PX4 it should be set to 100. On the Pixhawk powered from the PM connector it should be set to 2.
// @Values: -1:Disabled, 0:A0, 1:A1, 2:Pixhawk, 13:A13, 100:PX4
// @User: Standard
GSCALAR(battery_volt_pin, "BATT_VOLT_PIN", BATTERY_VOLT_PIN),
// @Param: BATT_CURR_PIN
// @DisplayName: Battery Current sensing pin
// @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 12. On the PX4 it should be set to 101. On the Pixhawk powered from the PM connector it should be set to 3.
// @Values: -1:Disabled, 1:A1, 2:A2, 3:Pixhawk, 12:A12, 101:PX4
// @User: Standard
GSCALAR(battery_curr_pin, "BATT_CURR_PIN", BATTERY_CURR_PIN),
// @Param: RSSI_PIN
// @DisplayName: Receiver RSSI sensing pin
// @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum
@ -888,6 +840,10 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(camera_mount2, "MNT2_", AP_Mount),
#endif
// @Group: BATT_
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
GOBJECT(battery, "BATT_", AP_BattMonitor),
#if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
// @Group: SIM_
// @Path: ../libraries/SITL/SITL.cpp

35
ArduPlane/config.h
View File

@ -89,14 +89,10 @@
// main board differences
//
#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
# define BATTERY_VOLT_PIN 0 // Battery voltage on A0
# define BATTERY_CURR_PIN 1 // Battery current on A1
# define CONFIG_INS_TYPE CONFIG_INS_OILPAN
# define CONFIG_BARO AP_BARO_BMP085
# define CONFIG_COMPASS AP_COMPASS_HMC5843
#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
# define BATTERY_VOLT_PIN 1 // Battery voltage on A1
# define BATTERY_CURR_PIN 2 // Battery current on A2
# define CONFIG_INS_TYPE CONFIG_INS_MPU6000
# ifdef APM2_BETA_HARDWARE
# define CONFIG_BARO AP_BARO_BMP085
@ -106,27 +102,15 @@
# endif
# define CONFIG_COMPASS AP_COMPASS_HMC5843
#elif CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
# define BATTERY_VOLT_PIN 1 // Battery voltage on A1
# define BATTERY_CURR_PIN 2 // Battery current on A2
# define CONFIG_INS_TYPE CONFIG_INS_HIL
# define CONFIG_BARO AP_BARO_HIL
# define CONFIG_COMPASS AP_COMPASS_HIL
#elif CONFIG_HAL_BOARD == HAL_BOARD_PX4
# define BATTERY_VOLT_PIN -1
# define BATTERY_CURR_PIN -1
# define CONFIG_INS_TYPE CONFIG_INS_PX4
# define CONFIG_BARO AP_BARO_PX4
# define CONFIG_COMPASS AP_COMPASS_PX4
# define SERIAL0_BAUD 115200
#elif CONFIG_HAL_BOARD == HAL_BOARD_FLYMAPLE
// Flymaple board pin 20 is connected to the external battery supply
// via a 24k/5.1k voltage divider. The schematic claims the divider is 25k/5k,
// but the actual installed resistors are not so.
// So the divider ratio is 5.70588 = (24000+5100)/5100
# define BATTERY_VOLT_PIN 20
# define BATTERY_MONITORING 3
# define VOLT_DIV_RATIO 5.70588
# define BATTERY_CURR_PIN 19
# define CONFIG_INS_TYPE CONFIG_INS_FLYMAPLE
# define CONFIG_BARO AP_BARO_BMP085
# define CONFIG_COMPASS AP_COMPASS_HMC5843
@ -191,25 +175,6 @@
#endif
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef BATTERY_MONITORING
#define BATTERY_MONITORING 0
#endif
#ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.56 // This is the proper value for an on-board APM1 voltage divider with a 3.9kOhm resistor
//# define VOLT_DIV_RATIO 15.70 // This is the proper value for the AttoPilot 50V/90A sensor
//# define VOLT_DIV_RATIO 4.127 // This is the proper value for the AttoPilot 13.6V/45A sensor
#endif
#ifndef CURR_AMP_PER_VOLT
# define CURR_AMP_PER_VOLT 27.32 // This is the proper value for the AttoPilot 50V/90A sensor
//# define CURR_AMP_PER_VOLT 13.66 // This is the proper value for the AttoPilot 13.6V/45A sensor
#endif
//////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION

6
ArduPlane/events.pde
View File

@ -104,14 +104,14 @@ static void failsafe_short_off_event()
void low_battery_event(void)
{
if (battery.low_batttery) {
if (failsafe.low_battery) {
return;
}
gcs_send_text_fmt(PSTR("Low Battery %.2fV Used %.0f mAh"),
battery.voltage, battery.current_total_mah);
battery.voltage(), battery.current_total_mah());
set_mode(RTL);
aparm.throttle_cruise.load();
battery.low_batttery = true;
failsafe.low_battery = true;
AP_Notify::flags.failsafe_battery = true;
}

32
ArduPlane/sensors.pde
View File

@ -40,42 +40,26 @@ static void zero_airspeed(void)
gcs_send_text_P(SEVERITY_LOW,PSTR("zero airspeed calibrated"));
}
// read_battery - reads battery voltage and current and invokes failsafe
// should be called at 10hz
static void read_battery(void)
{
if(g.battery_monitoring == 0) {
battery.voltage = 0;
if(battery.monitoring() == AP_BATT_MONITOR_DISABLED) {
return;
}
if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
// this copes with changing the pin at runtime
batt_volt_pin->set_pin(g.battery_volt_pin);
battery.voltage = BATTERY_VOLTAGE(batt_volt_pin);
}
if (g.battery_monitoring == 4) {
uint32_t tnow = hal.scheduler->millis();
float dt = tnow - battery.last_time_ms;
// this copes with changing the pin at runtime
batt_curr_pin->set_pin(g.battery_curr_pin);
battery.current_amps = CURRENT_AMPS(batt_curr_pin);
if (battery.last_time_ms != 0 && dt < 2000) {
// .0002778 is 1/3600 (conversion to hours)
battery.current_total_mah += battery.current_amps * dt * 0.0002778f;
}
battery.last_time_ms = tnow;
}
battery.read();
if (!usb_connected &&
battery.voltage != 0 &&
battery.voltage() != 0 &&
g.fs_batt_voltage > 0 &&
battery.voltage < g.fs_batt_voltage) {
battery.voltage() < g.fs_batt_voltage) {
low_battery_event();
}
if (!usb_connected &&
g.battery_monitoring == 4 &&
battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT &&
g.fs_batt_mah > 0 &&
g.pack_capacity - battery.current_total_mah < g.fs_batt_mah) {
battery.pack_capacity() - battery.current_total_mah() < g.fs_batt_mah) {
low_battery_event();
}
}

22
ArduPlane/setup.pde
View File

@ -15,7 +15,6 @@ static int8_t setup_set (uint8_t argc, const Men
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
static int8_t setup_batt_monitor (uint8_t argc, const Menu::arg *argv);
// Command/function table for the setup menu
@ -31,7 +30,6 @@ static const struct Menu::command setup_menu_commands[] PROGMEM = {
#endif
{"compass", setup_compass},
{"declination", setup_declination},
{"battery", setup_batt_monitor},
{"show", setup_show},
#if !defined( __AVR_ATmega1280__ )
{"set", setup_set},
@ -436,20 +434,6 @@ setup_compass(uint8_t argc, const Menu::arg *argv)
return 0;
}
static int8_t
setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
{
if(argv[1].i >= 0 && argv[1].i <= 4) {
g.battery_monitoring.set_and_save(argv[1].i);
} else {
cliSerial->printf_P(PSTR("\nOptions: 3-4"));
}
report_batt_monitor();
return 0;
}
/***************************************************************************/
// CLI reports
/***************************************************************************/
@ -459,9 +443,9 @@ static void report_batt_monitor()
//print_blanks(2);
cliSerial->printf_P(PSTR("Batt Mointor\n"));
print_divider();
if(g.battery_monitoring == 0) cliSerial->printf_P(PSTR("Batt monitoring disabled"));
if(g.battery_monitoring == 3) cliSerial->printf_P(PSTR("Monitoring batt volts"));
if(g.battery_monitoring == 4) cliSerial->printf_P(PSTR("Monitoring volts and current"));
if(battery.monitoring() == AP_BATT_MONITOR_DISABLED) cliSerial->printf_P(PSTR("Batt monitoring disabled"));
if(battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("Monitoring batt volts"));
if(battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("Monitoring volts and current"));
print_blanks(2);
}
static void report_radio()

16
ArduPlane/test.pde
View File

@ -252,23 +252,23 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
{
if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY || battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
print_hit_enter();
while(1) {
delay(100);
read_radio();
read_battery();
if (g.battery_monitoring == 3) {
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) {
cliSerial->printf_P(PSTR("V: %4.4f\n"),
battery.voltage,
battery.current_amps,
battery.current_total_mah);
battery.voltage(),
battery.current_amps(),
battery.current_total_mah());
} else {
cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
battery.voltage,
battery.current_amps,
battery.current_total_mah);
battery.voltage(),
battery.current_amps(),
battery.current_total_mah());
}
// write out the servo PWM values