AC sync battery code with arduplane

This commit is contained in:
Michael Oborne 2012-01-21 06:29:09 +08:00
parent 4eb8e8b172
commit 3b93418d67
3 changed files with 20 additions and 15 deletions

View File

@ -95,6 +95,9 @@ public:
//
k_param_IMU_calibration = 140,
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
@ -199,10 +201,12 @@ public:
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_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;
@ -320,10 +324,12 @@ public:
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")),
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")),

View File

@ -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
/* ************************************************************** */

View File

@ -106,12 +106,11 @@ static void read_battery(void)
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_total1 += current_amps1 * 0.0002778; // .0002778 is 1/3600 (conversion to hours)
}
#if BATTERY_EVENT == 1