Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Update battery monitoring code for ArduPlane 

Fixes compatibility for APM2.  Also a significant update to the battery monitoring code:  We previously had monitoring modes for individual cell voltages for 3 and 4 cell lipos.  These have been removed as they were never really supported (the cell voltages were computed but were not reported or recorded anywhere).  Also, some clean-up/prep work was done for supporting monitoring 2 separate battery packs.  The CLI battery and current monitoring tests were consolidated into 1 test.
This commit is contained in:
Doug Weibel 2012-01-15 16:10:28 -07:00
parent 9c5a776456
commit 69c7598db2
9 changed files with 56 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
ArduPlane/ArduPlane.pde
View File

@ -390,14 +390,14 @@ static float crosstrack_error; // meters we are off trackline
// Battery Sensors
// ---------------
static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter
static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter
static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery 1 Voltage, initialized above threshold for filter
static float current_amps1; // Current (Amperes) draw from battery 1
static float current_total1; // Totalized current (Amp-hours) from battery 1
static float current_amps;
static float current_total;
// To Do - Add support for second battery pack
//static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery 2 Voltage, initialized above threshold for filter
//static float current_amps2; // Current (Amperes) draw from battery 2
//static float current_total2; // Totalized current (Amp-hours) from battery 2
// Airspeed Sensors
// ----------------

14
ArduPlane/GCS_Mavlink.pde
View File

@ -206,11 +206,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
uint16_t battery_current = -1;
uint8_t battery_remaining = -1;
if (current_total != 0 && g.pack_capacity != 0) {
battery_remaining = (100.0 * (g.pack_capacity - current_total) / g.pack_capacity);
if (current_total1 != 0 && g.pack_capacity != 0) {
battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity);
}
if (current_total != 0) {
battery_current = current_amps * 100;
if (current_total1 != 0) {
battery_current = current_amps1 * 100;
}
mavlink_msg_sys_status_send(
@ -219,7 +219,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
control_sensors_enabled,
control_sensors_health,
(uint16_t)(load * 1000),
battery_voltage * 1000, // mV
battery_voltage1 * 1000, // mV
battery_current, // in 10mA units
battery_remaining, // in %
0, // comm drops %,
@ -270,7 +270,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
}
uint8_t status = MAV_STATE_ACTIVE;
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total1)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000
mavlink_msg_sys_status_send(
chan,
@ -278,7 +278,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
nav_mode,
status,
load * 1000,
battery_voltage * 1000,
battery_voltage1 * 1000,
battery_remaining,
packet_drops);
#endif // MAVLINK10

6
ArduPlane/Log.pde
View File

@ -386,9 +386,9 @@ static void Log_Write_Current()
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CURRENT_MSG);
DataFlash.WriteInt(g.channel_throttle.control_in);
DataFlash.WriteInt((int)(battery_voltage * 100.0));
DataFlash.WriteInt((int)(current_amps * 100.0));
DataFlash.WriteInt((int)current_total);
DataFlash.WriteInt((int)(battery_voltage1 * 100.0));
DataFlash.WriteInt((int)(current_amps1 * 100.0));
DataFlash.WriteInt((int)current_total1);
DataFlash.WriteByte(END_BYTE);
}

2
ArduPlane/Parameters.h
View File

@ -330,7 +330,7 @@ public:
AP_Int32 ground_pressure;
AP_Int8 compass_enabled;
AP_Int16 angle_of_attack;
AP_Int8 battery_monitoring; // 0=disabled, 1=3 cell lipo, 2=4 cell lipo, 3=total voltage only, 4=total voltage and current
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_Float input_voltage;

4
ArduPlane/config.h
View File

@ -87,6 +87,8 @@
# define PUSHBUTTON_PIN 41
# define USB_MUX_PIN -1
# define CONFIG_RELAY ENABLED
# define BATTERY_PIN_1 0
# define CURRENT_PIN_1 1
#elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
# define A_LED_PIN 27
# define B_LED_PIN 26
@ -97,6 +99,8 @@
# define PUSHBUTTON_PIN (-1)
# define CLI_SLIDER_ENABLED DISABLED
# define USB_MUX_PIN 23
# define BATTERY_PIN_1 1
# define CURRENT_PIN_1 2
#endif
//////////////////////////////////////////////////////////////////////////////

10
ArduPlane/defines.h
View File

@ -183,18 +183,8 @@ enum gcs_severity {
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor
#define BATTERY_PIN1 0 // These are the pins for the voltage dividers
#define BATTERY_PIN2 1
#define BATTERY_PIN3 2
#define BATTERY_PIN4 3
#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage
#define CURRENT_PIN_1 1 // and current
#define RELAY_PIN 47

25
ArduPlane/sensors.pde
View File

@ -106,25 +106,22 @@ static void zero_airspeed(void)
static void read_battery(void)
{
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN1)) * .1 + battery_voltage1 * .9;
battery_voltage2 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN2)) * .1 + battery_voltage2 * .9;
battery_voltage3 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN3)) * .1 + battery_voltage3 * .9;
battery_voltage4 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN4)) * .1 + battery_voltage4 * .9;
if(g.battery_monitoring == 1)
battery_voltage = battery_voltage3; // set total battery voltage, for telemetry stream
if(g.battery_monitoring == 2)
battery_voltage = battery_voltage4;
if(g.battery_monitoring == 0) {
battery_voltage1 = 0;
return;
}
if(g.battery_monitoring == 3 || g.battery_monitoring == 4)
battery_voltage = battery_voltage1;
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9;
Serial.print("Raw current: "); Serial.println(analogRead(CURRENT_PIN_1));
if(g.battery_monitoring == 4) {
current_amps = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps * .9; //reads power sensor current pin
current_total += current_amps * (float)delta_ms_medium_loop * 0.000278;
current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin
current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours)
}
#if BATTERY_EVENT == ENABLED
if(battery_voltage < LOW_VOLTAGE) low_battery_event();
if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event();
if(battery_voltage1 < LOW_VOLTAGE) low_battery_event();
if(g.battery_monitoring == 4 && current_total1 > g.pack_capacity) low_battery_event();
#endif
}

2
ArduPlane/setup.pde
View File

@ -336,8 +336,6 @@ static void report_batt_monitor()
Serial.printf_P(PSTR("Batt Mointor\n"));
print_divider();
if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled"));
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("Monitoring 3 cell"));
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("Monitoring 4 cell"));
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current"));
print_blanks(2);

60
ArduPlane/test.pde
View File

@ -14,7 +14,6 @@ static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
#endif
static int8_t test_imu(uint8_t argc, const Menu::arg *argv);
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
static int8_t test_current(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_airspeed(uint8_t argc, const Menu::arg *argv);
@ -34,18 +33,18 @@ static int8_t test_logging(uint8_t argc, const Menu::arg *argv);
// User enters the string in the console to call the functions on the right.
// See class Menu in AP_Common for implementation details
static const struct Menu::command test_menu_commands[] PROGMEM = {
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"passthru", test_passthru},
{"failsafe", test_failsafe},
{"battery", test_battery},
{"relay", test_relay},
{"waypoints", test_wp},
{"xbee", test_xbee},
{"eedump", test_eedump},
{"modeswitch", test_modeswitch},
{"pwm", test_radio_pwm},
{"radio", test_radio},
{"passthru", test_passthru},
{"failsafe", test_failsafe},
{"battery", test_battery},
{"relay", test_relay},
{"waypoints", test_wp},
{"xbee", test_xbee},
{"eedump", test_eedump},
{"modeswitch", test_modeswitch},
#if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2
{"dipswitches", test_dipswitches},
{"dipswitches", test_dipswitches},
#endif
// Tests below here are for hardware sensors only present
@ -60,7 +59,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
{"airspeed", test_airspeed},
{"airpressure", test_pressure},
{"compass", test_mag},
{"current", test_current},
#elif HIL_MODE == HIL_MODE_SENSORS
{"adc", test_adc},
{"gps", test_gps},
@ -260,26 +258,7 @@ 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 >=1 && g.battery_monitoring < 4) {
for (int i = 0; i < 80; i++){ // Need to get many samples for filter to stabilize
delay(20);
read_battery();
}
Serial.printf_P(PSTR("Volts: 1:%2.2f, 2:%2.2f, 3:%2.2f, 4:%2.2f\n"),
battery_voltage1,
battery_voltage2,
battery_voltage3,
battery_voltage4);
} else {
Serial.printf_P(PSTR("Not enabled\n"));
}
return (0);
}
static int8_t
test_current(uint8_t argc, const Menu::arg *argv)
{
if (g.battery_monitoring == 4) {
if (g.battery_monitoring == 3 || g.battery_monitoring == 4) {
print_hit_enter();
delta_ms_medium_loop = 100;
@ -287,10 +266,17 @@ if (g.battery_monitoring == 4) {
delay(100);
read_radio();
read_battery();
Serial.printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"),
battery_voltage,
current_amps,
current_total);
if (g.battery_monitoring == 3){
Serial.printf_P(PSTR("V: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
} else {
Serial.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
// ------------------------------