From 121e4932ea27942507af6c4a008c9500a946699c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 2 Oct 2013 15:07:28 +0900 Subject: [PATCH] Rover: integrate BattMonitor --- APMrover2/APMrover2.pde | 16 +++------- APMrover2/GCS_Mavlink.pde | 21 ++++--------- APMrover2/Log.pde | 6 ++-- APMrover2/Parameters.h | 19 ++++-------- APMrover2/Parameters.pde | 65 +++++++++++++++------------------------ APMrover2/config.h | 24 --------------- APMrover2/defines.h | 4 --- APMrover2/sensors.pde | 31 ++----------------- APMrover2/setup.pde | 30 +++++------------- APMrover2/test.pde | 39 ----------------------- 10 files changed, 54 insertions(+), 201 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index fd28d04897..36d55e9e67 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -103,6 +103,7 @@ #include "compat.h" #include // Notify library +#include // 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(); diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 201d26dfc8..7375cddfdf 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -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 %, diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index cca16dd5d5..ebdae2a8af 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -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)); } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index b8fdb0c68a..62e6ca4c7f 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -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 // @@ -189,13 +188,7 @@ public: AP_Int8 telem_delay; // 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 + AP_Int8 compass_enabled; // navigation parameters // diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index 9595488468..2dc108799f 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -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) { diff --git a/APMrover2/config.h b/APMrover2/config.h index d6c83aa228..cde9963470 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -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 // diff --git a/APMrover2/defines.h b/APMrover2/defines.h index d5d120a5bd..9f19e93684 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -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 diff --git a/APMrover2/sensors.pde b/APMrover2/sensors.pde index 34bc1896c8..3852b57e85 100644 --- a/APMrover2/sensors.pde +++ b/APMrover2/sensors.pde @@ -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) diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index 16b68f2e40..0ddfc71128 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -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() { diff --git a/APMrover2/test.pde b/APMrover2/test.pde index 1a9e59db51..5d13b1728d 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -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) {