diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 80147e64ea..e2bf52f9e4 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -94,7 +94,10 @@ public: // 140: Sensor parameters // k_param_IMU_calibration = 140, - k_param_battery_monitoring, + k_param_battery_monitoring, + k_param_volt_div_ratio, + k_param_curr_amp_per_volt, + k_param_input_voltage, k_param_pack_capacity, k_param_compass_enabled, k_param_compass, @@ -102,11 +105,10 @@ public: k_param_frame_orientation, k_param_top_bottom_ratio, k_param_optflow_enabled, - k_param_input_voltage, k_param_low_voltage, k_param_ch7_option, - k_param_sonar_type, // 153 - k_param_super_simple, + k_param_sonar_type, + k_param_super_simple, //155 // // 160: Navigation parameters @@ -198,11 +200,13 @@ 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, 3=voltage only, 4=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; AP_Int16 pack_capacity; // Battery pack capacity less reserve AP_Int8 compass_enabled; AP_Int8 optflow_enabled; - AP_Float input_voltage; AP_Float low_voltage; AP_Int8 super_simple; @@ -319,11 +323,13 @@ public: RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")), sonar_enabled (DISABLED, k_param_sonar, PSTR("SONAR_ENABLE")), sonar_type (AP_RANGEFINDER_MAXSONARXL, k_param_sonar_type, PSTR("SONAR_TYPE")), - battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), + battery_monitoring (DISABLED, k_param_battery_monitoring, PSTR("BATT_MONITOR")), + volt_div_ratio (VOLT_DIV_RATIO, k_param_volt_div_ratio, PSTR("VOLT_DIVIDER")), + curr_amp_per_volt (CURR_AMP_PER_VOLT, k_param_curr_amp_per_volt, PSTR("AMP_PER_VOLT")), + input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("INPUT_VOLTS")), pack_capacity (HIGH_DISCHARGE, k_param_pack_capacity, PSTR("BATT_CAPACITY")), compass_enabled (MAGNETOMETER, k_param_compass_enabled, PSTR("MAG_ENABLE")), optflow_enabled (OPTFLOW, k_param_optflow_enabled, PSTR("FLOW_ENABLE")), - input_voltage (INPUT_VOLTAGE, k_param_input_voltage, PSTR("IN_VOLT")), low_voltage (LOW_VOLTAGE, k_param_low_voltage, PSTR("LOW_VOLT")), super_simple (SUPER_SIMPLE, k_param_super_simple, PSTR("SUPER_SIMPLE")), diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index a9927a105c..6c0720799d 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -291,9 +291,9 @@ enum gcs_severity { // Climb rate calculations #define ALTITUDE_HISTORY_LENGTH 8 //Number of (time,altitude) points to regress a climb rate from +#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 BATTERY_VOLTAGE(x) (x*(g.input_voltage/1023.0))*VOLT_DIV_RATIO -#define CURRENT_AMPS(x) ((x*(g.input_voltage/1023.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT //#define BARO_FILTER_SIZE 8 /* ************************************************************** */ diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 2837ccd1c9..6b1cdcc112 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -105,13 +105,12 @@ static void read_battery(void) battery_voltage1 = 0; return; } - - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { + + 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_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) + current_amps1 = CURRENT_AMPS(analogRead(CURRENT_PIN_1)) * .1 + current_amps1 * .9; //reads power sensor current pin + current_total1 += current_amps1 * 0.0002778; // .0002778 is 1/3600 (conversion to hours) } #if BATTERY_EVENT == 1