mirror of https://github.com/ArduPilot/ardupilot
Rover: integrate BattMonitor
This commit is contained in:
Randy Mackay
parent
dab4f032f9
commit
121e4932ea
|
|
@ -103,6 +103,7 @@
|
|||
#include "compat.h"
|
||||
|
||||
#include <AP_Notify.h> // Notify library
|
||||
#include <AP_BattMonitor.h> // Battery monitor library
|
||||
|
||||
// Configuration
|
||||
#include "config.h"
|
||||
|
|
@ -274,9 +275,6 @@ AP_HAL::AnalogSource *rssi_analog_source;
|
|||
|
||||
AP_HAL::AnalogSource *vcc_pin;
|
||||
|
||||
AP_HAL::AnalogSource * batt_volt_pin;
|
||||
AP_HAL::AnalogSource * batt_curr_pin;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// SONAR selection
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
|
@ -444,13 +442,7 @@ static int8_t CH7_wp_index;
|
|||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Battery Sensors
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Battery pack 1 voltage. Initialized above the low voltage threshold to pre-load the filter and prevent low voltage events at startup.
|
||||
static float battery_voltage1 = LOW_VOLTAGE * 1.05;
|
||||
// Battery pack 1 instantaneous currrent draw. Amperes
|
||||
static float current_amps1;
|
||||
// Totalized current (Amp-hours) from battery 1
|
||||
static float current_total1;
|
||||
|
||||
static AP_BattMonitor battery;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation control variables
|
||||
|
|
@ -594,10 +586,10 @@ 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();
|
||||
|
||||
|
|
|
|||
|
|
@ -169,21 +169,12 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
|||
control_sensors_enabled &= ~(1<<2); // compass
|
||||
}
|
||||
|
||||
uint16_t battery_current = -1;
|
||||
uint8_t battery_remaining = -1;
|
||||
int16_t battery_current = -1;
|
||||
int8_t battery_remaining = -1;
|
||||
|
||||
if (current_total1 != 0 && g.pack_capacity != 0) {
|
||||
battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity);
|
||||
}
|
||||
if (current_total1 != 0) {
|
||||
battery_current = current_amps1 * 100;
|
||||
}
|
||||
|
||||
if (g.battery_monitoring == 3) {
|
||||
/*setting a out-of-range value.
|
||||
* It informs to external devices that
|
||||
* it cannot be calculated properly just by voltage*/
|
||||
battery_remaining = 150;
|
||||
if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
|
||||
battery_remaining = battery.capacity_remaining_pct();
|
||||
battery_current = battery.current_amps() * 100;
|
||||
}
|
||||
|
||||
mavlink_msg_sys_status_send(
|
||||
|
|
@ -192,7 +183,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_voltage1 * 1000, // mV
|
||||
battery.voltage() * 1000, // mV
|
||||
battery_current, // in 10mA units
|
||||
battery_remaining, // in %
|
||||
0, // comm drops %,
|
||||
|
|
|
|||
|
|
@ -407,10 +407,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_voltage1 * 100.0),
|
||||
current_amps : (int16_t)(current_amps1 * 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 : current_total1
|
||||
current_total : battery.current_total_mah()
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -58,11 +58,12 @@ public:
|
|||
k_param_steering_learn, // unused
|
||||
|
||||
// 140: battery controls
|
||||
k_param_battery_monitoring = 140,
|
||||
k_param_volt_div_ratio,
|
||||
k_param_curr_amp_per_volt,
|
||||
k_param_battery_monitoring = 140, // deprecated, can be deleted
|
||||
k_param_volt_div_ratio, // deprecated, can be deleted
|
||||
k_param_curr_amp_per_volt, // deprecated, can be deleted
|
||||
k_param_input_voltage, // deprecated, can be deleted
|
||||
k_param_pack_capacity,
|
||||
k_param_pack_capacity, // deprecated, can be deleted
|
||||
k_param_battery,
|
||||
|
||||
//
|
||||
// 150: Navigation parameters
|
||||
|
|
@ -177,8 +178,6 @@ public:
|
|||
|
||||
// IO pins
|
||||
AP_Int8 rssi_pin;
|
||||
AP_Int8 battery_volt_pin;
|
||||
AP_Int8 battery_curr_pin;
|
||||
|
||||
// Telemetry control
|
||||
//
|
||||
|
|
@ -191,12 +190,6 @@ public:
|
|||
// sensor parameters
|
||||
AP_Int8 compass_enabled;
|
||||
|
||||
// battery controls
|
||||
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
|
||||
AP_Float volt_div_ratio;
|
||||
AP_Float curr_amp_per_volt;
|
||||
AP_Int16 pack_capacity; // Battery pack capacity less reserve
|
||||
|
||||
// navigation parameters
|
||||
//
|
||||
AP_Float speed_cruise;
|
||||
|
|
|
|||
|
|
@ -41,20 +41,6 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
// @User: Standard
|
||||
GSCALAR(rssi_pin, "RSSI_PIN", -1),
|
||||
|
||||
// @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", 1),
|
||||
|
||||
// @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 by 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", 2),
|
||||
|
||||
// @Param: SYSID_THIS_MAV
|
||||
// @DisplayName: MAVLink system ID
|
||||
// @Description: ID used in MAVLink protocol to identify this vehicle
|
||||
|
|
@ -97,32 +83,6 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
// @Values: 0:Disabled,1:Enabled
|
||||
GSCALAR(compass_enabled, "MAG_ENABLE", MAGNETOMETER),
|
||||
|
||||
// @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", DISABLED),
|
||||
|
||||
// @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: AMP_PER_VOLT
|
||||
// @DisplayName: Current Amps per volt
|
||||
// @Description: Used to convert the voltage on the current sensing pin (BATT_CURR_PIN) to the actual current being consumed in amps. For the 3DR Power brick on APM2 or Pixhawk this should be set to 17
|
||||
// @User: Advanced
|
||||
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT),
|
||||
|
||||
// @Param: BATT_CAPACITY
|
||||
// @DisplayName: Battery Capacity
|
||||
// @Description: Battery capacity in milliamp-hours (mAh)
|
||||
// @Units: mAh
|
||||
// @User: Standard
|
||||
GSCALAR(pack_capacity, "BATT_CAPACITY", HIGH_DISCHARGE),
|
||||
|
||||
// @Param: AUTO_TRIGGER_PIN
|
||||
// @DisplayName: Auto mode trigger pin
|
||||
// @Description: pin number to use to enable the throttle in auto mode. If set to -1 then don't use a trigger, otherwise this is a pin number which if held low in auto mode will enable the motor to run. If the switch is released while in AUTO then the motor will stop again. This can be used in combination with INITIAL_MODE to give a 'press button to start' rover with no receiver.
|
||||
|
|
@ -493,9 +453,34 @@ const AP_Param::Info var_info[] PROGMEM = {
|
|||
GOBJECT(camera_mount, "MNT_", AP_Mount),
|
||||
#endif
|
||||
|
||||
// @Group: BATT_
|
||||
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
|
||||
GOBJECT(battery, "BATT_", AP_BattMonitor),
|
||||
|
||||
AP_VAREND
|
||||
};
|
||||
|
||||
/*
|
||||
This is a conversion table from old parameter values to new
|
||||
parameter names. The startup code looks for saved values of the old
|
||||
parameters and will copy them across to the new parameters if the
|
||||
new parameter does not yet have a saved value. It then saves the new
|
||||
value.
|
||||
|
||||
Note that this works even if the old parameter has been removed. It
|
||||
relies on the old k_param index not being removed
|
||||
|
||||
The second column below is the index in the var_info[] table for the
|
||||
old object. This should be zero for top level parameters.
|
||||
*/
|
||||
const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
|
||||
{ Parameters::k_param_battery_monitoring, 0, AP_PARAM_INT8, "BATT_MONITOR" },
|
||||
{ Parameters::k_param_battery_volt_pin, 0, AP_PARAM_INT8, "BATT_VOLT_PIN" },
|
||||
{ Parameters::k_param_battery_curr_pin, 0, AP_PARAM_INT8, "BATT_CURR_PIN" },
|
||||
{ Parameters::k_param_volt_div_ratio, 0, AP_PARAM_FLOAT, "BATT_VOLT_MULT" },
|
||||
{ Parameters::k_param_curr_amp_per_volt, 0, AP_PARAM_FLOAT, "BATT_AMP_PERVOLT" },
|
||||
{ Parameters::k_param_pack_capacity, 0, AP_PARAM_INT32, "BATT_CAPACITY" },
|
||||
};
|
||||
|
||||
static void load_parameters(void)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -144,30 +144,6 @@
|
|||
# define TUNING_OPTION TUN_NONE
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Battery monitoring
|
||||
//
|
||||
#ifndef BATTERY_EVENT
|
||||
# define BATTERY_EVENT DISABLED
|
||||
#endif
|
||||
#ifndef LOW_VOLTAGE
|
||||
# define LOW_VOLTAGE 9.6
|
||||
#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
|
||||
#endif
|
||||
|
||||
#ifndef CURR_AMP_PER_VOLT
|
||||
# define CURR_AMP_PER_VOLT 27.32 // This is the proper value for the AttoPilot 50V/90A sensor
|
||||
#endif
|
||||
|
||||
#ifndef CURR_AMPS_OFFSET
|
||||
# define CURR_AMPS_OFFSET 0.0
|
||||
#endif
|
||||
#ifndef HIGH_DISCHARGE
|
||||
# define HIGH_DISCHARGE 1760
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// INPUT_VOLTAGE
|
||||
//
|
||||
|
|
|
|||
|
|
@ -164,10 +164,6 @@ enum ap_message {
|
|||
// Climb rate calculations
|
||||
#define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from
|
||||
|
||||
|
||||
#define BATTERY_VOLTAGE(x) (x->voltage_average()*g.volt_div_ratio)
|
||||
#define CURRENT_AMPS(x) (x->voltage_average()-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
|
||||
|
||||
#define RELAY_PIN 47
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -11,38 +11,13 @@ static void init_sonar(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
read and update the battery
|
||||
*/
|
||||
// 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_voltage1 = 0;
|
||||
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_voltage1 = BATTERY_VOLTAGE(batt_volt_pin);
|
||||
}
|
||||
|
||||
if (g.battery_monitoring == 4) {
|
||||
static uint32_t last_time_ms;
|
||||
uint32_t tnow = hal.scheduler->millis();
|
||||
float dt = tnow - last_time_ms;
|
||||
if (last_time_ms != 0 && dt < 2000) {
|
||||
// this copes with changing the pin at runtime
|
||||
batt_curr_pin->set_pin(g.battery_curr_pin);
|
||||
current_amps1 = CURRENT_AMPS(batt_curr_pin);
|
||||
// .0002778 is 1/3600 (conversion to hours)
|
||||
current_total1 += current_amps1 * dt * 0.0002778f;
|
||||
}
|
||||
last_time_ms = tnow;
|
||||
}
|
||||
battery.read();
|
||||
}
|
||||
|
||||
|
||||
// read the receiver RSSI as an 8 bit number for MAVLink
|
||||
// RC_CHANNELS_SCALED message
|
||||
void read_receiver_rssi(void)
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@ static int8_t setup_level (uint8_t argc, const Menu::arg *argv);
|
|||
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
|
||||
static const struct Menu::command setup_menu_commands[] PROGMEM = {
|
||||
|
|
@ -30,7 +29,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},
|
||||
|
|
@ -460,33 +458,19 @@ 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
|
||||
/***************************************************************************/
|
||||
|
||||
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"));
|
||||
print_blanks(2);
|
||||
//print_blanks(2);
|
||||
cliSerial->printf_P(PSTR("Batt Mointor\n"));
|
||||
print_divider();
|
||||
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()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -10,7 +10,6 @@ static int8_t test_passthru(uint8_t argc, const Menu::arg *argv);
|
|||
static int8_t test_failsafe(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_ins(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t test_sonar(uint8_t argc, const Menu::arg *argv);
|
||||
|
|
@ -30,7 +29,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
|
|||
{"radio", test_radio},
|
||||
{"passthru", test_passthru},
|
||||
{"failsafe", test_failsafe},
|
||||
{"battery", test_battery},
|
||||
{"relay", test_relay},
|
||||
{"waypoints", test_wp},
|
||||
{"modeswitch", test_modeswitch},
|
||||
|
|
@ -211,43 +209,6 @@ 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) {
|
||||
print_hit_enter();
|
||||
|
||||
while(1){
|
||||
delay(100);
|
||||
read_radio();
|
||||
read_battery();
|
||||
if (g.battery_monitoring == 3){
|
||||
cliSerial->printf_P(PSTR("V: %4.4f\n"),
|
||||
battery_voltage1,
|
||||
current_amps1,
|
||||
current_total1);
|
||||
} else {
|
||||
cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
|
||||
battery_voltage1,
|
||||
current_amps1,
|
||||
current_total1);
|
||||
}
|
||||
|
||||
// write out the servo PWM values
|
||||
// ------------------------------
|
||||
set_servos();
|
||||
|
||||
if(cliSerial->available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
cliSerial->printf_P(PSTR("Not enabled\n"));
|
||||
return (0);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static int8_t
|
||||
test_relay(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue