diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 291992ae4e..6e9037014d 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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; diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 195c9b40c2..6e2e6715dc 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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); } diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index e0d2acab55..57b2136ff0 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -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); } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 4895a39b52..80147e64ea 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -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; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index c662b0baa0..bcf285fd51 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index ff8f628bf9..a9927a105c 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 21692965bd..2837ccd1c9 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -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 diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 0d714d62b2..3b7ed57455 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -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); diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index b965847a89..e1fc53df4d 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -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); diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 3b46f10af6..49dd6f9ca7 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -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();