diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 321917f6b2..53f5676c9f 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -135,6 +135,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 // @@ -335,6 +341,8 @@ public: AP_Float curr_amp_per_volt; AP_Float input_voltage; AP_Int32 pack_capacity; // Battery pack capacity less reserve + AP_Int8 battery_volt_pin; + AP_Int8 battery_curr_pin; AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger AP_Int8 stick_mixing; AP_Int8 rudder_steer; diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 201b5f4f2c..fc8596706c 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -530,6 +530,20 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT), GSCALAR(input_voltage, "INPUT_VOLTS", INPUT_VOLTAGE), GSCALAR(pack_capacity, "BATT_CAPACITY", HIGH_DISCHARGE), + + // @Param: BATT_VOLT_PIN + // @DisplayName: Battery Voltage sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. + // @Values: 99:Disabled, 0:A0, 1:A1, 13:A13 + // @User: Standard + GSCALAR(battery_volt_pin, "BATT_VOLT_PIN", BATTERY_VOLT_PIN), + + // @Param: BATT_CURR_PIN + // @DisplayName: Battery Current sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. + // @Values: 99:Disabled, 1:A1, 2:A2, 12:A12 + // @User: Standard + GSCALAR(battery_curr_pin, "BATT_CURR_PIN", BATTERY_CURR_PIN), GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH", 0), // barometer ground calibration. The GND_ prefix is chosen for diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 45cb4160c6..13261e2fcd 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -116,8 +116,8 @@ # define LED_OFF LOW # define USB_MUX_PIN -1 # define CONFIG_RELAY ENABLED - # define BATTERY_PIN_1 0 - # define CURRENT_PIN_1 1 + # 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 @@ -129,8 +129,8 @@ #else # define USB_MUX_PIN 23 #endif - # define BATTERY_PIN_1 1 - # define CURRENT_PIN_1 2 + # define BATTERY_VOLT_PIN 1 // Battery voltage on A1 + # define BATTERY_CURR_PIN 2 // Battery current on A2 #endif diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 940617d4aa..e584d33b3e 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -40,12 +40,12 @@ static void read_battery(void) } if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { - static AP_AnalogSource_Arduino bat_pin(BATTERY_PIN_1); - 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(CURRENT_PIN_1); - current_amps1 = CURRENT_AMPS(current_pin.read_average()); + static AP_AnalogSource_Arduino batt_curr_pin(g.battery_curr_pin); + current_amps1 = CURRENT_AMPS(batt_curr_pin.read_average()); current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) }