diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 6e45378b47..2d774804f0 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -390,14 +390,14 @@ static float crosstrack_error; // meters we are off trackline // Battery Sensors // --------------- -static float battery_voltage = LOW_VOLTAGE * 1.05; // Battery Voltage of total battery, initialized above threshold for filter -static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery Voltage of cell 1, initialized above threshold for filter -static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2, initialized above threshold for filter -static float battery_voltage3 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3, initialized above threshold for filter -static float battery_voltage4 = LOW_VOLTAGE * 1.05; // Battery Voltage of cells 1 + 2+3 + 4, initialized above threshold for filter +static float battery_voltage1 = LOW_VOLTAGE * 1.05; // Battery 1 Voltage, initialized above threshold for filter +static float current_amps1; // Current (Amperes) draw from battery 1 +static float current_total1; // Totalized current (Amp-hours) from battery 1 -static float current_amps; -static float current_total; +// To Do - Add support for second battery pack +//static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery 2 Voltage, initialized above threshold for filter +//static float current_amps2; // Current (Amperes) draw from battery 2 +//static float current_total2; // Totalized current (Amp-hours) from battery 2 // Airspeed Sensors // ---------------- diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index ee4810e14d..3bf8642db8 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -206,11 +206,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack uint16_t battery_current = -1; uint8_t battery_remaining = -1; - if (current_total != 0 && g.pack_capacity != 0) { - battery_remaining = (100.0 * (g.pack_capacity - current_total) / g.pack_capacity); + if (current_total1 != 0 && g.pack_capacity != 0) { + battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity); } - if (current_total != 0) { - battery_current = current_amps * 100; + if (current_total1 != 0) { + battery_current = current_amps1 * 100; } mavlink_msg_sys_status_send( @@ -219,7 +219,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack control_sensors_enabled, control_sensors_health, (uint16_t)(load * 1000), - battery_voltage * 1000, // mV + battery_voltage1 * 1000, // mV battery_current, // in 10mA units battery_remaining, // in % 0, // comm drops %, @@ -270,7 +270,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack } uint8_t status = MAV_STATE_ACTIVE; - 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, @@ -278,7 +278,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack nav_mode, status, load * 1000, - battery_voltage * 1000, + battery_voltage1 * 1000, battery_remaining, packet_drops); #endif // MAVLINK10 diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 6b9887c076..da073b0873 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -386,9 +386,9 @@ static void Log_Write_Current() DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_CURRENT_MSG); DataFlash.WriteInt(g.channel_throttle.control_in); - DataFlash.WriteInt((int)(battery_voltage * 100.0)); - DataFlash.WriteInt((int)(current_amps * 100.0)); - DataFlash.WriteInt((int)current_total); + DataFlash.WriteInt((int)(battery_voltage1 * 100.0)); + DataFlash.WriteInt((int)(current_amps1 * 100.0)); + DataFlash.WriteInt((int)current_total1); DataFlash.WriteByte(END_BYTE); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 9277351bf9..c618b634c1 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -330,7 +330,7 @@ public: AP_Int32 ground_pressure; AP_Int8 compass_enabled; AP_Int16 angle_of_attack; - 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_Float volt_div_ratio; AP_Float curr_amp_per_volt; AP_Float input_voltage; diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 0d599e4d43..4acb5238f7 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -87,6 +87,8 @@ # define PUSHBUTTON_PIN 41 # define USB_MUX_PIN -1 # define CONFIG_RELAY ENABLED +# 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 @@ -97,6 +99,8 @@ # define PUSHBUTTON_PIN (-1) # define CLI_SLIDER_ENABLED DISABLED # define USB_MUX_PIN 23 +# define BATTERY_PIN_1 1 +# define CURRENT_PIN_1 2 #endif ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 3c365f68eb..67afa10cb2 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -183,18 +183,8 @@ enum gcs_severity { #define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0))*g.volt_div_ratio - #define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt -#define AIRSPEED_CH 7 // The external ADC channel for the airspeed sensor -#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 VOLTAGE_PIN_0 0 // These are the pins for current sensor: voltage -#define CURRENT_PIN_1 1 // and current - #define RELAY_PIN 47 diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index cb96f16e2b..c878c0194c 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -106,25 +106,22 @@ static void zero_airspeed(void) 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 == 0) { + battery_voltage1 = 0; + return; + } + if(g.battery_monitoring == 3 || g.battery_monitoring == 4) - battery_voltage = battery_voltage1; + battery_voltage1 = BATTERY_VOLTAGE(analogRead(BATTERY_PIN_1)) * .1 + battery_voltage1 * .9; +Serial.print("Raw current: "); Serial.println(analogRead(CURRENT_PIN_1)); 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 * (float)delta_ms_medium_loop * 0.000278; + current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin + current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) } #if BATTERY_EVENT == ENABLED - if(battery_voltage < LOW_VOLTAGE) low_battery_event(); - if(g.battery_monitoring == 4 && current_total > g.pack_capacity) low_battery_event(); + if(battery_voltage1 < LOW_VOLTAGE) low_battery_event(); + if(g.battery_monitoring == 4 && current_total1 > g.pack_capacity) low_battery_event(); #endif } diff --git a/ArduPlane/setup.pde b/ArduPlane/setup.pde index 2841e8b787..134cc516f0 100644 --- a/ArduPlane/setup.pde +++ b/ArduPlane/setup.pde @@ -336,8 +336,6 @@ static void report_batt_monitor() Serial.printf_P(PSTR("Batt Mointor\n")); print_divider(); if(g.battery_monitoring == 0) Serial.printf_P(PSTR("Batt monitoring disabled")); - if(g.battery_monitoring == 1) Serial.printf_P(PSTR("Monitoring 3 cell")); - if(g.battery_monitoring == 2) Serial.printf_P(PSTR("Monitoring 4 cell")); if(g.battery_monitoring == 3) Serial.printf_P(PSTR("Monitoring batt volts")); if(g.battery_monitoring == 4) Serial.printf_P(PSTR("Monitoring volts and current")); print_blanks(2); diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index ebe7d76091..105d343ea1 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -14,7 +14,6 @@ static int8_t test_adc(uint8_t argc, const Menu::arg *argv); #endif static int8_t test_imu(uint8_t argc, const Menu::arg *argv); static int8_t test_battery(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); static int8_t test_airspeed(uint8_t argc, const Menu::arg *argv); @@ -34,18 +33,18 @@ static int8_t test_logging(uint8_t argc, const Menu::arg *argv); // User enters the string in the console to call the functions on the right. // See class Menu in AP_Common for implementation details static const struct Menu::command test_menu_commands[] PROGMEM = { - {"pwm", test_radio_pwm}, - {"radio", test_radio}, - {"passthru", test_passthru}, - {"failsafe", test_failsafe}, - {"battery", test_battery}, - {"relay", test_relay}, - {"waypoints", test_wp}, - {"xbee", test_xbee}, - {"eedump", test_eedump}, - {"modeswitch", test_modeswitch}, + {"pwm", test_radio_pwm}, + {"radio", test_radio}, + {"passthru", test_passthru}, + {"failsafe", test_failsafe}, + {"battery", test_battery}, + {"relay", test_relay}, + {"waypoints", test_wp}, + {"xbee", test_xbee}, + {"eedump", test_eedump}, + {"modeswitch", test_modeswitch}, #if CONFIG_APM_HARDWARE != APM_HARDWARE_APM2 - {"dipswitches", test_dipswitches}, + {"dipswitches", test_dipswitches}, #endif // Tests below here are for hardware sensors only present @@ -60,7 +59,6 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { {"airspeed", test_airspeed}, {"airpressure", test_pressure}, {"compass", test_mag}, - {"current", test_current}, #elif HIL_MODE == HIL_MODE_SENSORS {"adc", test_adc}, {"gps", test_gps}, @@ -260,26 +258,7 @@ 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 >=1 && g.battery_monitoring < 4) { - for (int i = 0; i < 80; i++){ // Need to get many samples for filter to stabilize - 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")); -} - return (0); -} - -static int8_t -test_current(uint8_t argc, const Menu::arg *argv) -{ -if (g.battery_monitoring == 4) { +if (g.battery_monitoring == 3 || g.battery_monitoring == 4) { print_hit_enter(); delta_ms_medium_loop = 100; @@ -287,10 +266,17 @@ if (g.battery_monitoring == 4) { 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, mAh: %4.4f\n"), + battery_voltage1, + current_amps1, + current_total1); + } // write out the servo PWM values // ------------------------------