From 4a41a3d210bd028d8aaefe8145ae206ba31102de Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Sat, 13 Oct 2012 18:40:46 +0900 Subject: [PATCH] ArduCopter: added BATT_VOLT_PIN and BATT_CURR_PIN parameters to allow support for new 3DR IV battery voltage and current monitor --- ArduCopter/Parameters.h | 15 ++++++++++----- ArduCopter/Parameters.pde | 24 +++++++++++++----------- ArduCopter/config.h | 6 ++++-- ArduCopter/sensors.pde | 8 ++++---- 4 files changed, 31 insertions(+), 22 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e631a0b7c3..42ee26e52b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -114,9 +114,8 @@ public: k_param_ch7_option, k_param_auto_slew_rate, k_param_sonar_type, - k_param_super_simple, - k_param_battery_pin, - k_param_axis_enabled, + k_param_super_simple = 155, + k_param_axis_enabled = 157, k_param_copter_leds_mode, k_param_ahrs, // AHRS group @@ -137,6 +136,12 @@ public: k_param_camera_mount, k_param_camera_mount2, + // + // Batery monitoring parameters + // + k_param_battery_volt_pin = 168, + k_param_battery_curr_pin, // 169 + // // 170: Radio settings // @@ -237,14 +242,14 @@ public: AP_Int8 optflow_enabled; AP_Float low_voltage; AP_Int8 super_simple; - AP_Int8 battery_pin; AP_Int16 rtl_approach_alt; AP_Int8 tilt_comp; AP_Int8 axis_enabled; AP_Int8 copter_leds_mode; // Operating mode of LED // lighting system - + AP_Int8 battery_volt_pin; + AP_Int8 battery_curr_pin; // Waypoints // diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 4562a695d1..143d187315 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -98,13 +98,6 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(super_simple, "SUPER_SIMPLE", SUPER_SIMPLE), - // @Param: BATT_PIN - // @DisplayName: Battery Voltage sending pin - // @Description: Setting this to 0 ~ 11 will enable battery voltage sending on pins A0 ~ A11. Current will be measured on this pin + 1 - // @Values: 99:Disabled, 0:A0, 1:A1, 10:A10 - // @User: Standard - GSCALAR(battery_pin, "BATT_PIN", BATTERY_PIN_1), - // @Param: APPROACH_ALT // @DisplayName: RTL Approach Altitude // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. @@ -122,10 +115,19 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Advanced GSCALAR(tilt_comp, "TILT", TILT_COMPENSATION), - - - - + // @Param: BATT_VOLT_PIN + // @DisplayName: Battery Voltage sending pin + // @Description: Setting this to 0 ~ 11 will enable battery voltage sending on pins A0 ~ A11. Current will be measured on this pin + 1 + // @Values: 99:Disabled, 0:A0, 1:A1, 10:A13 + // @User: Standard + GSCALAR(battery_volt_pin, "BATT_VOLT_PIN", BATTERY_VOLT_PIN), + + // @Param: BATT_CURR_PIN + // @DisplayName: Battery Voltage sending pin + // @Description: Setting this to 0 ~ 11 will enable battery voltage sending on pins A0 ~ A11. Current will be measured on this pin + 1 + // @Values: 99:Disabled, 0:A0, 1:A1, 10:A12 + // @User: Standard + GSCALAR(battery_curr_pin, "BATT_CURR_PIN", BATTERY_CURR_PIN), GSCALAR(waypoint_mode, "WP_MODE", 0), GSCALAR(command_total, "WP_TOTAL", 0), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index af6bf049cf..b2521866bd 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -140,7 +140,8 @@ # define USB_MUX_PIN -1 # define CLI_SLIDER_ENABLED DISABLED # define OPTFLOW_CS_PIN 34 - # define BATTERY_PIN_1 0 // Battery voltage on A0, Current on A1 + # define BATTERY_VOLT_PIN 0 // Battery voltage on A0 + # define BATTERY_CURR_PIN 1 // Battery current on A1 #elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 # define A_LED_PIN 27 # define B_LED_PIN 26 @@ -152,7 +153,8 @@ # define CLI_SLIDER_ENABLED DISABLED # define USB_MUX_PIN 23 # define OPTFLOW_CS_PIN A3 - # define BATTERY_PIN_1 1 // Battery voltage on A1, Current on A2 + # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 + # define BATTERY_CURR_PIN 2 // Battery current on A2 #endif //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 921e92580d..1623db0f61 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -77,12 +77,12 @@ static void read_battery(void) } if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino bat_pin(g.battery_pin); - battery_voltage1 = BATTERY_VOLTAGE(bat_pin.read_average()); + static AP_AnalogSource_Arduino batt_volt_pin(g.battery_volt_pin); + battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin.read_average()); } if(g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino current_pin(g.battery_pin+1); // current is always read from one pin higher than battery voltage - current_amps1 = CURRENT_AMPS(current_pin.read_average()); + static AP_AnalogSource_Arduino batt_curr_pin(g.battery_curr_pin); // current is always read from one pin higher than battery voltage + current_amps1 = CURRENT_AMPS(batt_curr_pin.read_average()); current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours) }