From ea4f256f8e7d7aeedd3ae1a707e08c46bd3e5fae Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Tue, 2 Oct 2012 22:16:19 +0900 Subject: [PATCH] ArduCopter: BATT_PIN parameter added to allow you to select which pin is used for voltage and current measurements To save a parameter, the current sensor pin is assumed to always be 1 higher than the voltage pin. --- ArduCopter/Parameters.h | 6 ++++-- ArduCopter/Parameters.pde | 7 +++++++ ArduCopter/config.h | 6 ++---- ArduCopter/sensors.pde | 4 ++-- 4 files changed, 15 insertions(+), 8 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 8d0306d851..e631a0b7c3 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -114,8 +114,9 @@ public: k_param_ch7_option, k_param_auto_slew_rate, k_param_sonar_type, - k_param_super_simple = 155, - k_param_axis_enabled = 157, + k_param_super_simple, + k_param_battery_pin, + k_param_axis_enabled, k_param_copter_leds_mode, k_param_ahrs, // AHRS group @@ -236,6 +237,7 @@ 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; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 5c4fdd58bf..6833078504 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -98,6 +98,13 @@ 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. diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 1df71d87ba..009114b933 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -140,8 +140,7 @@ # 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 + # define BATTERY_PIN_1 0 // Battery voltage on A0, Current on A1 #elif CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 # define A_LED_PIN 27 # define B_LED_PIN 26 @@ -153,8 +152,7 @@ # define CLI_SLIDER_ENABLED DISABLED # define USB_MUX_PIN 23 # define OPTFLOW_CS_PIN A3 - # define BATTERY_PIN_1 1 - # define CURRENT_PIN_1 2 + # define BATTERY_PIN_1 1 // Battery voltage on A1, Current on A2 #endif //////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 53df0e4cfa..921e92580d 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -77,11 +77,11 @@ static void read_battery(void) } if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino bat_pin(BATTERY_PIN_1); + static AP_AnalogSource_Arduino bat_pin(g.battery_pin); battery_voltage1 = BATTERY_VOLTAGE(bat_pin.read_average()); } if(g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino current_pin(CURRENT_PIN_1); + 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()); current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours) }