Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: integrate BattMonitor

Browse Source
This commit is contained in:
Randy Mackay 2013-10-01 22:34:44 +09:00
parent 9722276827
commit dab4f032f9
13 changed files with 108 additions and 215 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/AP_State.pde
View File

@ -71,7 +71,7 @@ static void set_failsafe_radio(bool b)
// --------------------------------------------- // ---------------------------------------------
void set_low_battery(bool b) void set_low_battery(bool b)
{ {
ap.low_battery = b; failsafe.low_battery = b;
AP_Notify::flags.failsafe_battery = b; AP_Notify::flags.failsafe_battery = b;
} }

40
ArduCopter/ArduCopter.pde
View File

@ -117,6 +117,7 @@
#include <AP_Scheduler.h> // main loop scheduler #include <AP_Scheduler.h> // main loop scheduler
#include <AP_RCMapper.h> // RC input mapping library #include <AP_RCMapper.h> // RC input mapping library
#include <AP_Notify.h> // Notify library #include <AP_Notify.h> // Notify library
#include <AP_BattMonitor.h> // Battery monitor library
#if SPRAYER == ENABLED #if SPRAYER == ENABLED
#include <AC_Sprayer.h> // crop sprayer library #include <AC_Sprayer.h> // crop sprayer library
#endif #endif
@ -375,19 +376,18 @@ static AP_RangeFinder_MaxsonarXL *sonar;
static union { static union {
struct { struct {
uint8_t home_is_set : 1; // 0 uint8_t home_is_set : 1; // 0
uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE uint8_t simple_mode : 2; // 1,2 // This is the state of simple mode : 0 = disabled ; 1 = SIMPLE ; 2 = SUPERSIMPLE
uint8_t pre_arm_rc_check : 1; // 5 // true if rc input pre-arm checks have been completed successfully uint8_t pre_arm_rc_check : 1; // 3 // true if rc input pre-arm checks have been completed successfully
uint8_t pre_arm_check : 1; // 6 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed uint8_t pre_arm_check : 1; // 4 // true if all pre-arm checks (rc, accel calibration, gps lock) have been performed
uint8_t auto_armed : 1; // 7 // stops auto missions from beginning until throttle is raised uint8_t auto_armed : 1; // 5 // stops auto missions from beginning until throttle is raised
uint8_t logging_started : 1; // 8 // true if dataflash logging has started uint8_t logging_started : 1; // 6 // true if dataflash logging has started
uint8_t low_battery : 1; // 9 // Used to track if the battery is low - LED output flashes when the batt is low uint8_t do_flip : 1; // 7 // Used to enable flip code
uint8_t do_flip : 1; // 15 // Used to enable flip code uint8_t takeoff_complete : 1; // 8
uint8_t takeoff_complete : 1; // 16 uint8_t land_complete : 1; // 9 // true if we have detected a landing
uint8_t land_complete : 1; // 17 // true if we have detected a landing uint8_t compass_status : 1; // 10 // unused remove
uint8_t compass_status : 1; // 18 uint8_t gps_status : 1; // 11 // unused remove
uint8_t gps_status : 1; // 19
}; };
uint32_t value; uint32_t value;
} ap; } ap;
@ -425,7 +425,7 @@ static uint8_t receiver_rssi;
static struct { static struct {
uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station
uint8_t radio : 1; // 1 // A status flag for the radio failsafe uint8_t radio : 1; // 1 // A status flag for the radio failsafe
uint8_t batt : 1; // 2 // A status flag for the battery failsafe uint8_t low_battery : 1; // 2 // A status flag for the battery failsafe
uint8_t gps : 1; // 3 // A status flag for the gps failsafe uint8_t gps : 1; // 3 // A status flag for the gps failsafe
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
@ -624,12 +624,7 @@ static int8_t aux_switch_wp_index;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Battery Sensors // Battery Sensors
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Battery Voltage of battery, initialized above threshold for filter static AP_BattMonitor battery;
static float battery_voltage1 = LOW_VOLTAGE * 1.05f;
// refers to the instant amp draw – based on an Attopilot Current sensor
static float current_amps1;
// refers to the total amps drawn – based on an Attopilot Current sensor
static float current_total1;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -899,6 +894,9 @@ void setup() {
// initialise notify system // initialise notify system
notify.init(); notify.init();
// initialise battery monitor
battery.init();
#if CONFIG_SONAR == ENABLED #if CONFIG_SONAR == ENABLED
#if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC #if CONFIG_SONAR_SOURCE == SONAR_SOURCE_ADC
sonar_analog_source = new AP_ADC_AnalogSource( sonar_analog_source = new AP_ADC_AnalogSource(
@ -914,8 +912,6 @@ void setup() {
#endif #endif
rssi_analog_source = hal.analogin->channel(g.rssi_pin); rssi_analog_source = hal.analogin->channel(g.rssi_pin);
batt_volt_analog_source = hal.analogin->channel(g.battery_volt_pin);
batt_curr_analog_source = hal.analogin->channel(g.battery_curr_pin);
board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC); board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
init_ardupilot(); init_ardupilot();
@ -1126,9 +1122,7 @@ static void medium_loop()
medium_loopCounter++; medium_loopCounter++;
// read battery before compass because it may be used for motor interference compensation // read battery before compass because it may be used for motor interference compensation
if (g.battery_monitoring != 0) { read_battery();
read_battery();
}
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
if(g.compass_enabled) { if(g.compass_enabled) {

16
ArduCopter/GCS_Mavlink.pde
View File

@ -189,17 +189,15 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
control_sensors_enabled &= ~(1<<2); // compass control_sensors_enabled &= ~(1<<2); // compass
} }
uint16_t battery_current = -1; int16_t battery_current = -1;
uint8_t battery_remaining = -1; int8_t battery_remaining = -1;
if (current_total1 != 0 && g.pack_capacity != 0) { if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
battery_remaining = (100.0f * (g.pack_capacity - current_total1) / g.pack_capacity); battery_remaining = battery.capacity_remaining_pct();
} battery_current = battery.current_amps() * 100;
if (current_total1 != 0) {
battery_current = current_amps1 * 100;
} }
if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) { if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) {
/*setting a out-of-range value. /*setting a out-of-range value.
* It informs to external devices that * It informs to external devices that
* it cannot be calculated properly just by voltage*/ * it cannot be calculated properly just by voltage*/
@ -212,7 +210,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
control_sensors_enabled, control_sensors_enabled,
control_sensors_health, control_sensors_health,
(uint16_t)(scheduler.load_average(10000) * 1000), (uint16_t)(scheduler.load_average(10000) * 1000),
battery_voltage1 * 1000, // mV battery.voltage() * 1000, // mV
battery_current, // in 10mA units battery_current, // in 10mA units
battery_remaining, // in % battery_remaining, // in %
0, // comm drops %, 0, // comm drops %,

6
ArduCopter/Log.pde
View File

@ -177,10 +177,10 @@ static void Log_Write_Current()
LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
throttle_in : g.rc_3.control_in, throttle_in : g.rc_3.control_in,
throttle_integrator : throttle_integrator, throttle_integrator : throttle_integrator,
battery_voltage : (int16_t) (battery_voltage1 * 100.0f), battery_voltage : (int16_t) (battery.voltage() * 100.0f),
current_amps : (int16_t) (current_amps1 * 100.0f), current_amps : (int16_t) (battery.current_amps() * 100.0f),
board_voltage : board_voltage(), board_voltage : board_voltage(),
current_total : current_total1 current_total : battery.current_total_mah()
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }

32
ArduCopter/Parameters.h
View File

@ -90,7 +90,9 @@ public:
k_param_arming_check_enabled, k_param_arming_check_enabled,
k_param_sprayer, k_param_sprayer,
k_param_angle_max, k_param_angle_max,
k_param_gps_hdop_good, // 35 k_param_gps_hdop_good,
k_param_battery,
k_param_fs_batt_mah, // 37
// 65: AP_Limits Library // 65: AP_Limits Library
k_param_limits = 65, // deprecated - remove k_param_limits = 65, // deprecated - remove
@ -135,24 +137,24 @@ public:
// 140: Sensor parameters // 140: Sensor parameters
// //
k_param_imu = 140, // deprecated - can be deleted k_param_imu = 140, // deprecated - can be deleted
k_param_battery_monitoring = 141, k_param_battery_monitoring = 141, // deprecated - can be deleted
k_param_volt_div_ratio, k_param_volt_div_ratio, // deprecated - can be deleted
k_param_curr_amp_per_volt, k_param_curr_amp_per_volt, // deprecated - can be deleted
k_param_input_voltage, // deprecated - can be deleted k_param_input_voltage, // deprecated - can be deleted
k_param_pack_capacity, k_param_pack_capacity, // deprecated - can be deleted
k_param_compass_enabled, k_param_compass_enabled,
k_param_compass, k_param_compass,
k_param_sonar_enabled, k_param_sonar_enabled,
k_param_frame_orientation, k_param_frame_orientation,
k_param_optflow_enabled, k_param_optflow_enabled,
k_param_low_voltage, k_param_fs_batt_voltage,
k_param_ch7_option, k_param_ch7_option,
k_param_auto_slew_rate, // deprecated - can be deleted k_param_auto_slew_rate, // deprecated - can be deleted
k_param_sonar_type, k_param_sonar_type,
k_param_super_simple = 155, k_param_super_simple = 155,
k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change k_param_axis_enabled = 157, // deprecated - remove with next eeprom number change
k_param_copter_leds_mode, k_param_copter_leds_mode,
k_param_ahrs, // AHRS group k_param_ahrs, // AHRS group // 159
// //
// 160: Navigation parameters // 160: Navigation parameters
@ -174,8 +176,8 @@ public:
// //
// Batery monitoring parameters // Batery monitoring parameters
// //
k_param_battery_volt_pin = 168, k_param_battery_volt_pin = 168, // deprecated - can be deleted
k_param_battery_curr_pin, // 169 k_param_battery_curr_pin, // 169 deprecated - can be deleted
// //
// 170: Radio settings // 170: Radio settings
@ -281,26 +283,22 @@ public:
// 2 = XLL (XL with 10m range) // 2 = XLL (XL with 10m range)
// 3 = HRLV // 3 = HRLV
AP_Float sonar_gain; AP_Float sonar_gain;
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_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered
AP_Float fs_batt_mah; // battery capacity (in mah) below which failsafe will be triggered
AP_Int8 failsafe_gps_enabled; // gps failsafe enabled AP_Int8 failsafe_gps_enabled; // gps failsafe enabled
AP_Int8 failsafe_gcs; // ground station failsafe behavior AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int16 gps_hdop_good; // GPS Hdop value below which represent a good position AP_Int16 gps_hdop_good; // GPS Hdop value below which represent a good position
AP_Int8 compass_enabled; AP_Int8 compass_enabled;
AP_Int8 optflow_enabled; AP_Int8 optflow_enabled;
AP_Float low_voltage;
AP_Int8 super_simple; AP_Int8 super_simple;
AP_Int16 rtl_alt_final; AP_Int16 rtl_alt_final;
AP_Int8 copter_leds_mode; // Operating mode of LED AP_Int8 copter_leds_mode; // Operating mode of LED
// lighting system // lighting system
AP_Int8 battery_volt_pin;
AP_Int8 battery_curr_pin;
AP_Int8 rssi_pin; AP_Int8 rssi_pin;
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
AP_Int16 angle_max; // maximum lean angle of the copter in centi-degrees AP_Int16 angle_max; // maximum lean angle of the copter in centi-degrees

87
ArduCopter/Parameters.pde
View File

@ -95,13 +95,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard // @User: Standard
GSCALAR(sonar_gain, "SONAR_GAIN", SONAR_GAIN_DEFAULT), GSCALAR(sonar_gain, "SONAR_GAIN", SONAR_GAIN_DEFAULT),
// @Param: BATT_MONITOR
// @DisplayName: Battery monitoring
// @Description: Controls enabling monitoring of the battery's voltage and current
// @Values: 0:Disabled,3:Voltage Only,4:Voltage and Current
// @User: Standard
GSCALAR(battery_monitoring, "BATT_MONITOR", BATT_MONITOR_DISABLED),
// @Param: FS_BATT_ENABLE // @Param: FS_BATT_ENABLE
// @DisplayName: Battery Failsafe Enable // @DisplayName: Battery Failsafe Enable
// @Description: Controls whether failsafe will be invoked when battery voltage or current runs low // @Description: Controls whether failsafe will be invoked when battery voltage or current runs low
@ -109,6 +102,20 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard // @User: Standard
GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATTERY), GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATTERY),
// @Param: FS_BATT_VOLTAGE
// @DisplayName: Failsafe battery voltage
// @Description: Battery voltage to trigger failsafe. Set to 0 to disable battery voltage failsafe. If the battery voltage drops below this voltage then the plane will RTL
// @Units: Volts
// @User: Standard
GSCALAR(fs_batt_voltage, "FS_BATT_VOLTAGE", FS_BATT_VOLTAGE_DEFAULT),
// @Param: FS_BATT_MAH
// @DisplayName: Failsafe battery milliAmpHours
// @Description: Battery capacity remaining to trigger failsafe. Set to 0 to disable battery remaining failsafe. If the battery remaining drops below this level then the plane will RTL
// @Units: mAh
// @User: Standard
GSCALAR(fs_batt_mah, "FS_BATT_MAH", FS_BATT_MAH_DEFAULT),
// @Param: FS_GPS_ENABLE // @Param: FS_GPS_ENABLE
// @DisplayName: GPS Failsafe Enable // @DisplayName: GPS Failsafe Enable
// @Description: Controls whether failsafe will be invoked when gps signal is lost // @Description: Controls whether failsafe will be invoked when gps signal is lost
@ -130,25 +137,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Advanced // @User: Advanced
GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT), GSCALAR(gps_hdop_good, "GPS_HDOP_GOOD", GPS_HDOP_GOOD_DEFAULT),
// @Param: VOLT_DIVIDER
// @DisplayName: Voltage Divider
// @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_DIVIDER). For the 3DR Power brick on APM2 or Pixhawk this should be set to 10.1. For the PX4 using the PX4IO power supply this should be set to 1.
// @User: Advanced
GSCALAR(volt_div_ratio, "VOLT_DIVIDER", VOLT_DIV_RATIO),
// @Param: AMP_PER_VOLT
// @DisplayName: Current Amps per volt
// @Description: Used to convert the voltage on the current sensing pin (BATT_CURR_PIN) to the actual current being consumed in amps. For the 3DR Power brick on APM2 or Pixhawk it should be set to 17.
// @User: Advanced
GSCALAR(curr_amp_per_volt, "AMP_PER_VOLT", CURR_AMP_PER_VOLT),
// @Param: BATT_CAPACITY
// @DisplayName: Battery Capacity
// @Description: Battery capacity in milliamp-hours (mAh)
// @Units: mAh
// @User: Standard
GSCALAR(pack_capacity, "BATT_CAPACITY", BATTERY_CAPACITY_DEFAULT),
// @Param: MAG_ENABLE // @Param: MAG_ENABLE
// @DisplayName: Enable Compass // @DisplayName: Enable Compass
// @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass // @Description: Setting this to Enabled(1) will enable the compass. Setting this to Disabled(0) will disable the compass
@ -163,14 +151,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard // @User: Standard
GSCALAR(optflow_enabled, "FLOW_ENABLE", DISABLED), GSCALAR(optflow_enabled, "FLOW_ENABLE", DISABLED),
// @Param: LOW_VOLT
// @DisplayName: Low Voltage
// @Description: Set this to the voltage you want to represent low voltage
// @Range: 0 20
// @Increment: 0.1
// @User: Standard
GSCALAR(low_voltage, "LOW_VOLT", LOW_VOLTAGE),
// @Param: SUPER_SIMPLE // @Param: SUPER_SIMPLE
// @DisplayName: Enable Super Simple Mode // @DisplayName: Enable Super Simple Mode
// @Description: Setting this to Enabled(1) will enable Super Simple Mode. Setting this to Disabled(0) will disable Super Simple Mode // @Description: Setting this to Enabled(1) will enable Super Simple Mode. Setting this to Disabled(0) will disable Super Simple Mode
@ -187,20 +167,6 @@ const AP_Param::Info var_info[] PROGMEM = {
// @User: Standard // @User: Standard
GSCALAR(rtl_alt_final, "RTL_ALT_FINAL", RTL_ALT_FINAL), GSCALAR(rtl_alt_final, "RTL_ALT_FINAL", RTL_ALT_FINAL),
// @Param: BATT_VOLT_PIN
// @DisplayName: Battery Voltage sensing pin
// @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 13. On the PX4 it should be set to 100. On the Pixhawk powered from the PM connector it should be set to 2.
// @Values: -1:Disabled, 0:A0, 1:A1, 2:Pixhawk, 13:A13, 100:PX4
// @User: Standard
GSCALAR(battery_volt_pin, "BATT_VOLT_PIN", BATTERY_VOLT_PIN),
// @Param: BATT_CURR_PIN
// @DisplayName: Battery Current sensing pin
// @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. For the 3DR power brick on APM2.5 it should be set to 12. On the PX4 it should be set to 101. On the Pixhawk powered from the PM connector it should be set to 3.
// @Values: -1:Disabled, 1:A1, 2:A2, 3:Pixhawk, 12:A12, 101:PX4
// @User: Standard
GSCALAR(battery_curr_pin, "BATT_CURR_PIN", BATTERY_CURR_PIN),
// @Param: RSSI_PIN // @Param: RSSI_PIN
// @DisplayName: Receiver RSSI sensing pin // @DisplayName: Receiver RSSI sensing pin
// @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum // @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum
@ -1015,6 +981,10 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(camera_mount2, "MNT2_", AP_Mount), GOBJECT(camera_mount2, "MNT2_", AP_Mount),
#endif #endif
// @Group: BATT_
// @Path: ../libraries/AP_BattMonitor/AP_BattMonitor.cpp
GOBJECT(battery, "BATT_", AP_BattMonitor),
#if SPRAYER == ENABLED #if SPRAYER == ENABLED
// @Group: SPRAYER_ // @Group: SPRAYER_
// @Path: ../libraries/AC_Sprayer/AC_Sprayer.cpp // @Path: ../libraries/AC_Sprayer/AC_Sprayer.cpp
@ -1060,6 +1030,27 @@ const AP_Param::Info var_info[] PROGMEM = {
AP_VAREND AP_VAREND
}; };
/*
This is a conversion table from old parameter values to new
parameter names. The startup code looks for saved values of the old
parameters and will copy them across to the new parameters if the
new parameter does not yet have a saved value. It then saves the new
value.
Note that this works even if the old parameter has been removed. It
relies on the old k_param index not being removed
The second column below is the index in the var_info[] table for the
old object. This should be zero for top level parameters.
*/
const AP_Param::ConversionInfo conversion_table[] PROGMEM = {
{ Parameters::k_param_battery_monitoring, 0, AP_PARAM_INT8, "BATT_MONITOR" },
{ Parameters::k_param_battery_volt_pin, 0, AP_PARAM_INT8, "BATT_VOLT_PIN" },
{ Parameters::k_param_battery_curr_pin, 0, AP_PARAM_INT8, "BATT_CURR_PIN" },
{ Parameters::k_param_volt_div_ratio, 0, AP_PARAM_FLOAT, "BATT_VOLT_MULT" },
{ Parameters::k_param_curr_amp_per_volt, 0, AP_PARAM_FLOAT, "BATT_AMP_PERVOLT" },
{ Parameters::k_param_pack_capacity, 0, AP_PARAM_INT32, "BATT_CAPACITY" },
};
static void load_parameters(void) static void load_parameters(void)
{ {

17
ArduCopter/config.h
View File

@ -349,21 +349,12 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Battery monitoring // Battery monitoring
// //
#ifndef LOW_VOLTAGE #ifndef FS_BATT_VOLTAGE_DEFAULT
# define LOW_VOLTAGE 10.5f # define FS_BATT_VOLTAGE_DEFAULT 10.5f // default battery voltage below which failsafe will be triggered
#endif
#ifndef VOLT_DIV_RATIO
# define VOLT_DIV_RATIO 3.56f
#endif #endif
#ifndef CURR_AMP_PER_VOLT #ifndef FS_BATT_MAH_DEFAULT
# define CURR_AMP_PER_VOLT 27.32f # define FS_BATT_MAH_DEFAULT 0 // default battery capacity (in mah) below which failsafe will be triggered
#endif
#ifndef CURR_AMPS_OFFSET
# define CURR_AMPS_OFFSET 0.0f
#endif
#ifndef BATTERY_CAPACITY_DEFAULT
# define BATTERY_CAPACITY_DEFAULT 3500
#endif #endif
#ifndef BOARD_VOLTAGE_MIN #ifndef BOARD_VOLTAGE_MIN

8
ArduCopter/defines.h
View File

@ -333,14 +333,6 @@ enum ap_message {
#define DATA_SET_SIMPLE_OFF 27 #define DATA_SET_SIMPLE_OFF 27
#define DATA_SET_SUPERSIMPLE_ON 28 #define DATA_SET_SUPERSIMPLE_ON 28
// battery monitoring macros
#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
#define BATT_MONITOR_DISABLED 0
#define BATT_MONITOR_VOLTAGE_ONLY 3
#define BATT_MONITOR_VOLTAGE_AND_CURRENT 4
/* ************************************************************** */ /* ************************************************************** */
/* Expansion PIN's that people can use for various things. */ /* Expansion PIN's that people can use for various things. */

9
ArduCopter/events.pde
View File

@ -50,7 +50,7 @@ static void failsafe_radio_on_event()
break; break;
case LAND: case LAND:
// continue to land if battery failsafe is also active otherwise fall through to default handling // continue to land if battery failsafe is also active otherwise fall through to default handling
if (g.failsafe_battery_enabled && ap.low_battery) { if (g.failsafe_battery_enabled && failsafe.low_battery) {
break; break;
} }
default: default:
@ -85,8 +85,13 @@ static void failsafe_radio_off_event()
static void low_battery_event(void) static void low_battery_event(void)
{ {
// return immediately if low battery event has already been triggered
if (failsafe.low_battery) {
return;
}
// failsafe check // failsafe check
if (g.failsafe_battery_enabled && !ap.low_battery && motors.armed()) { if (g.failsafe_battery_enabled && motors.armed()) {
switch(control_mode) { switch(control_mode) {
case STABILIZE: case STABILIZE:
case ACRO: case ACRO:

2
ArduCopter/leds.pde
View File

@ -60,7 +60,7 @@ static void update_copter_leds(void)
// motor leds control // motor leds control
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_ENABLED) { if (g.copter_leds_mode & COPTER_LEDS_BITMASK_ENABLED) {
if (motors.armed()) { if (motors.armed()) {
if (ap.low_battery) { if (failsafe.low_battery) {
if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BATT_OSCILLATE) { if (g.copter_leds_mode & COPTER_LEDS_BITMASK_BATT_OSCILLATE) {
copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink copter_leds_oscillate(); //if motors are armed, but battery level is low, motor leds fast blink
} else { } else {

38
ArduCopter/sensors.pde
View File

@ -99,45 +99,19 @@ static void init_optflow()
// read_battery - check battery voltage and current and invoke failsafe if necessary // read_battery - check battery voltage and current and invoke failsafe if necessary
// called at 10hz // called at 10hz
#define BATTERY_FS_COUNTER 100 // 100 iterations at 10hz is 10 seconds
static void read_battery(void) static void read_battery(void)
{ {
static uint8_t low_battery_counter = 0; battery.read();
if(g.battery_monitoring == BATT_MONITOR_DISABLED) { // update compass with current value
battery_voltage1 = 0; if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
return; compass.set_current(battery.current_amps());
}
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY || g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) {
batt_volt_analog_source->set_pin(g.battery_volt_pin);
battery_voltage1 = BATTERY_VOLTAGE(batt_volt_analog_source);
}
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) {
static uint32_t last_time_ms;
uint32_t tnow = hal.scheduler->millis();
float dt_millis = tnow - last_time_ms;
current_amps1 = CURRENT_AMPS(batt_curr_analog_source);
if (last_time_ms != 0 && dt_millis < 2000) {
batt_curr_analog_source->set_pin(g.battery_curr_pin);
current_total1 += current_amps1 * 1000 * dt_millis * (1.0f/1000) * (1.0f/3600); //amps * amps to milliamps * milliseconds * milliseconds to seconds * seconds to hours
}
// update compass with current value
compass.set_current(current_amps1);
last_time_ms = tnow;
} }
// check for low voltage or current if the low voltage check hasn't already been triggered // check for low voltage or current if the low voltage check hasn't already been triggered
// we only check when we're not powered by USB to avoid false alarms during bench tests // we only check when we're not powered by USB to avoid false alarms during bench tests
if (!ap_system.usb_connected && !ap.low_battery && ( battery_voltage1 < g.low_voltage || (g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT && current_total1 > g.pack_capacity))) { if (!ap_system.usb_connected && !failsafe.low_battery && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
low_battery_counter++; low_battery_event();
if( low_battery_counter >= BATTERY_FS_COUNTER ) {
low_battery_counter = BATTERY_FS_COUNTER; // ensure counter does not overflow
low_battery_event();
}
}else{
// reset low_battery_counter in case it was a temporary voltage dip
low_battery_counter = 0;
} }
} }

16
ArduCopter/setup.pde
View File

@ -127,7 +127,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
bool updated = false; // have we updated the compensation vector at least once bool updated = false; // have we updated the compensation vector at least once
// default compensation type to use current if possible // default compensation type to use current if possible
if( g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT ) { if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) {
comp_type = AP_COMPASS_MOT_COMP_CURRENT; comp_type = AP_COMPASS_MOT_COMP_CURRENT;
}else{ }else{
comp_type = AP_COMPASS_MOT_COMP_THROTTLE; comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
@ -265,10 +265,10 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
updated = true; updated = true;
}else{ }else{
// current based compensation if more than 3amps being drawn // current based compensation if more than 3amps being drawn
motor_impact_scaled = motor_impact / current_amps1; motor_impact_scaled = motor_impact / battery.current_amps();
// adjust the motor compensation to negate the impact if drawing over 3amps // adjust the motor compensation to negate the impact if drawing over 3amps
if( current_amps1 >= 3.0f ) { if( battery.current_amps() >= 3.0f ) {
motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f; motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f;
updated = true; updated = true;
} }
@ -276,7 +276,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
// record maximum throttle and current // record maximum throttle and current
throttle_pct_max = max(throttle_pct_max, throttle_pct); throttle_pct_max = max(throttle_pct_max, throttle_pct);
current_amps_max = max(current_amps_max, current_amps1); current_amps_max = max(current_amps_max, battery.current_amps());
// display output at 1hz if throttle is above zero // display output at 1hz if throttle is above zero
print_counter++; print_counter++;
@ -334,7 +334,7 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv)
static void display_compassmot_info(Vector3f& motor_impact, Vector3f& motor_compensation) static void display_compassmot_info(Vector3f& motor_impact, Vector3f& motor_compensation)
{ {
// print one more time so the last thing printed matches what appears in the report_compass // print one more time so the last thing printed matches what appears in the report_compass
cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)current_amps1, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z); cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)battery.current_amps(), (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z);
} }
static int8_t static int8_t
@ -967,9 +967,9 @@ static void report_batt_monitor()
{ {
cliSerial->printf_P(PSTR("\nBatt Mon:\n")); cliSerial->printf_P(PSTR("\nBatt Mon:\n"));
print_divider(); print_divider();
if(g.battery_monitoring == BATT_MONITOR_DISABLED) print_enabled(false); if (battery.monitoring() == AP_BATT_MONITOR_DISABLED) print_enabled(false);
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("volts")); if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("volts"));
if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("volts and cur")); if (battery.monitoring() == AP_BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("volts and cur"));
print_blanks(2); print_blanks(2);
} }

50
ArduCopter/test.pde
View File

@ -7,7 +7,6 @@
#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS #if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS
static int8_t test_baro(uint8_t argc, const Menu::arg *argv); static int8_t test_baro(uint8_t argc, const Menu::arg *argv);
#endif #endif
static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
static int8_t test_compass(uint8_t argc, const Menu::arg *argv); static int8_t test_compass(uint8_t argc, const Menu::arg *argv);
static int8_t test_gps(uint8_t argc, const Menu::arg *argv); static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
static int8_t test_ins(uint8_t argc, const Menu::arg *argv); static int8_t test_ins(uint8_t argc, const Menu::arg *argv);
@ -32,7 +31,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = {
#if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS #if HIL_MODE != HIL_MODE_ATTITUDE && HIL_MODE != HIL_MODE_SENSORS
{"baro", test_baro}, {"baro", test_baro},
#endif #endif
{"battery", test_battery},
{"compass", test_compass}, {"compass", test_compass},
{"gps", test_gps}, {"gps", test_gps},
{"ins", test_ins}, {"ins", test_ins},
@ -88,54 +86,6 @@ test_baro(uint8_t argc, const Menu::arg *argv)
} }
#endif #endif
static int8_t
test_battery(uint8_t argc, const Menu::arg *argv)
{
// check if radio is calibration
pre_arm_rc_checks();
if(!ap.pre_arm_rc_check) {
cliSerial->print_P(PSTR("radio not calibrated, exiting"));
return(0);
}
cliSerial->printf_P(PSTR("\nCareful! Motors will spin! Press Enter to start.\n"));
while (cliSerial->read() != -1); /* flush */
while(!cliSerial->available()) { /* wait for input */
delay(100);
}
while (cliSerial->read() != -1); /* flush */
print_hit_enter();
// allow motors to spin
output_min();
motors.armed(true);
while(1) {
delay(100);
read_radio();
read_battery();
if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) {
cliSerial->printf_P(PSTR("V: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
} else {
cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, Ah: %4.4f\n"),
battery_voltage1,
current_amps1,
current_total1);
}
motors.throttle_pass_through();
if(cliSerial->available() > 0) {
motors.armed(false);
return (0);
}
}
motors.armed(false);
return (0);
}
static int8_t static int8_t
test_compass(uint8_t argc, const Menu::arg *argv) test_compass(uint8_t argc, const Menu::arg *argv)
{ {