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 ArduCopter 

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.
changed
This commit is contained in:
Doug Weibel 2012-01-15 17:10:02 -07:00
parent 762a5550de
commit ed9f7cb1b6
10 changed files with 38 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduCopter/ArduCopter.pde
View File

@ -530,20 +530,12 @@ static int8_t CH7_wp_index;
////////////////////////////////////////////////////////////////////////////////
// Battery Sensors
////////////////////////////////////////////////////////////////////////////////
// Battery Voltage of total battery, initialized above threshold for filter
static float battery_voltage = LOW_VOLTAGE * 1.05;
// Battery Voltage of cell 1, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05;
// Battery Voltage of cells 1 + 2, initialized above threshold for filter
static float battery_voltage2 = LOW_VOLTAGE * 1.05;
// Battery Voltage of cells 1 + 2+3, initialized above threshold for filter
static float battery_voltage3 = LOW_VOLTAGE * 1.05;
// Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter
static float battery_voltage4 = LOW_VOLTAGE * 1.05;
// Battery Voltage of battery, initialized above threshold for filter
static float battery_voltage1 = LOW_VOLTAGE * 1.05;
// refers to the instant amp draw – based on an Attopilot Current sensor
static float current_amps;
static float current_amps1;
// refers to the total amps drawn – based on an Attopilot Current sensor
static float current_total;
static float current_total1;
// Used to track if the battery is low - LED output flashes when the batt is low
static bool low_batt = false;

4
ArduCopter/GCS_Mavlink.pde
View File

@ -78,7 +78,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
status = MAV_STATE_STANDBY;
}
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,
@ -86,7 +86,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
nav_mode,
status,
0,
battery_voltage * 1000,
battery_voltage1 * 1000,
battery_remaining,
packet_drops);
}

6
ArduCopter/Log.pde
View File

@ -321,9 +321,9 @@ static void Log_Write_Current()
DataFlash.WriteInt(g.rc_3.control_in); // 1
DataFlash.WriteLong(throttle_integrator); // 2
DataFlash.WriteInt(battery_voltage * 100.0); // 3
DataFlash.WriteInt(current_amps * 100.0); // 4
DataFlash.WriteInt(current_total); // 5
DataFlash.WriteInt(battery_voltage1 * 100.0); // 3
DataFlash.WriteInt(current_amps1 * 100.0); // 4
DataFlash.WriteInt(current_total1); // 5
DataFlash.WriteByte(END_BYTE);
}

2
ArduCopter/Parameters.h
View File

@ -198,7 +198,7 @@ public:
AP_Int16 RTL_altitude;
AP_Int8 sonar_enabled;
AP_Int8 sonar_type; // 0 = XL, 1 = LV, 2 = XLL (XL with 10m range)
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_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 compass_enabled;
AP_Int8 optflow_enabled;

4
ArduCopter/config.h
View File

@ -118,6 +118,8 @@
# define USB_MUX_PIN -1
# define CLI_SLIDER_ENABLED DISABLED
# define OPTFLOW_CS_PIN 34
# 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
@ -129,6 +131,8 @@
# define CLI_SLIDER_ENABLED DISABLED
# define USB_MUX_PIN 23
# define OPTFLOW_CS_PIN A6
# define BATTERY_PIN_1 1
# define CURRENT_PIN_1 2
#endif
//////////////////////////////////////////////////////////////////////////////

7
ArduCopter/defines.h
View File

@ -325,15 +325,8 @@ enum gcs_severity {
#define AN14 68 // NC
#define AN15 69 // NC
#define VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage
#define CURRENT_PIN_1 1 // and current
#define RELAY_PIN 47
#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 PIEZO_PIN AN5 //Last pin on the back ADC connector

25
ArduCopter/sensors.pde
View File

@ -100,30 +100,25 @@ static void init_optflow()
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 == 3 || g.battery_monitoring == 4)
battery_voltage = battery_voltage1;
if(g.battery_monitoring == 0){
battery_voltage1 = 0;
return;
}
if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9;
}
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 * 0.0278; // called at 100ms on average
current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin
current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours)
}
#if BATTERY_EVENT == 1
//if(battery_voltage < g.low_voltage)
// low_battery_event();
if((battery_voltage < g.low_voltage) || (g.battery_monitoring == 4 && current_total > g.pack_capacity)){
if((battery_voltage1 < g.low_voltage) || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity)){
low_battery_event();
#if PIEZO_LOW_VOLTAGE == 1

4
ArduCopter/setup.pde
View File

@ -395,7 +395,7 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
g.battery_monitoring.set_and_save(argv[1].i);
} else {
Serial.printf_P(PSTR("\nOp: off, 1-4"));
Serial.printf_P(PSTR("\nOp: off, 3-4"));
}
report_batt_monitor();
@ -778,8 +778,6 @@ static void report_batt_monitor()
Serial.printf_P(PSTR("\nBatt Mon:\n"));
print_divider();
if(g.battery_monitoring == 0) print_enabled(false);
if(g.battery_monitoring == 1) Serial.printf_P(PSTR("3c"));
if(g.battery_monitoring == 2) Serial.printf_P(PSTR("4c"));
if(g.battery_monitoring == 3) Serial.printf_P(PSTR("volts"));
if(g.battery_monitoring == 4) Serial.printf_P(PSTR("volts and cur"));
print_blanks(2);

40
ArduCopter/test.pde
View File

@ -21,7 +21,6 @@ static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
//static int8_t test_wp_nav(uint8_t argc, const Menu::arg *argv);
//static int8_t test_reverse(uint8_t argc, const Menu::arg *argv);
static int8_t test_tuning(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);
#if HIL_MODE != HIL_MODE_ATTITUDE
@ -69,7 +68,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
{"battery", test_battery},
{"tune", test_tuning},
//{"tri", test_tri},
{"current", test_current},
{"relay", test_relay},
{"wp", test_wp},
//{"nav", test_nav},
@ -713,26 +711,6 @@ test_gps(uint8_t argc, const Menu::arg *argv)
}
//*/
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
{
#if BATTERY_EVENT == 1
for (int i = 0; i < 20; i++){
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"));
#endif
return (0);
}
static int8_t
test_tuning(uint8_t argc, const Menu::arg *argv)
{
@ -751,7 +729,7 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
}
static int8_t
test_current(uint8_t argc, const Menu::arg *argv)
test_battery(uint8_t argc, const Menu::arg *argv)
{
print_hit_enter();
//delta_ms_medium_loop = 100;
@ -760,11 +738,17 @@ test_current(uint8_t argc, const Menu::arg *argv)
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, Ah: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
}
APM_RC.OutputCh(MOT_1, g.rc_3.radio_in);
APM_RC.OutputCh(MOT_2, g.rc_3.radio_in);
APM_RC.OutputCh(MOT_3, g.rc_3.radio_in);

2
ArduPlane/setup.pde
View File

@ -322,7 +322,7 @@ setup_batt_monitor(uint8_t argc, const Menu::arg *argv)
g.battery_monitoring.set_and_save(argv[1].i);
} else {
Serial.printf_P(PSTR("\nOptions: 0-4"));
Serial.printf_P(PSTR("\nOptions: 3-4"));
}
report_batt_monitor();