mirror of https://github.com/ArduPilot/ardupilot
Copter: use voltage_average() so as to avoid INPUT_VOLTS
This commit is contained in:
parent
2331c84423
commit
c2fd1512ff
|
@ -347,8 +347,8 @@ enum gcs_severity {
|
|||
#define DATA_RTL_REACHED_ALT 31
|
||||
|
||||
// battery monitoring macros
|
||||
#define BATTERY_VOLTAGE(x) (x*(g.input_voltage/1024.0f))*g.volt_div_ratio
|
||||
#define CURRENT_AMPS(x) ((x*(g.input_voltage/1024.0f))-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
|
||||
#define BATTERY_VOLTAGE(x) (x->voltage_average()*g.volt_div_ratio)
|
||||
#define CURRENT_AMPS(x) (x->voltage_average()-CURR_AMPS_OFFSET)*g.curr_amp_per_volt
|
||||
|
||||
/* ************************************************************** */
|
||||
/* Expansion PIN's that people can use for various things. */
|
||||
|
|
|
@ -118,11 +118,11 @@ static void read_battery(void)
|
|||
|
||||
if(g.battery_monitoring == 3 || g.battery_monitoring == 4) {
|
||||
batt_volt_analog_source->set_pin(g.battery_volt_pin);
|
||||
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_analog_source->read_average());
|
||||
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_analog_source);
|
||||
}
|
||||
if(g.battery_monitoring == 4) {
|
||||
batt_curr_analog_source->set_pin(g.battery_curr_pin);
|
||||
current_amps1 = CURRENT_AMPS(batt_curr_analog_source->read_average());
|
||||
current_amps1 = CURRENT_AMPS(batt_curr_analog_source);
|
||||
current_total1 += current_amps1 * 0.02778f; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours)
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue