aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter: low baterry failsafe

Browse Source
This commit is contained in:
rmackay9 2012-11-13 23:43:54 +09:00
parent 73ed6c9dbf
commit e92b560df5
5 changed files with 66 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/Parameters.h
View File

@ -174,6 +174,7 @@ public:
k_param_radio_tuning_high,
k_param_radio_tuning_low,
k_param_rc_speed = 192,
k_param_battery_fs_enabled, // 193
//
// 200: flight modes
@ -246,8 +247,9 @@ public:
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_Int16 pack_capacity; // Battery pack capacity less reserve
AP_Int8 battery_fs_enabled; // battery failsafe enabled
AP_Int8 compass_enabled;
AP_Int8 optflow_enabled;
AP_Float low_voltage;

8
ArduCopter/Parameters.pde
View File

@ -53,8 +53,16 @@ const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(sonar_enabled, "SONAR_ENABLE", DISABLED),
GSCALAR(sonar_type, "SONAR_TYPE", AP_RANGEFINDER_MAXSONARXL),
GSCALAR(battery_monitoring, "BATT_MONITOR", DISABLED),
// @Param: BATT_FAILSAFE
// @DisplayName: Battery Failsafe Enable
// @Description: Controls whether failsafe will be invoked when battery voltage or current runs low
// @Values: 0:Disabled,1:Enabled
// @User: Standard
GSCALAR(battery_fs_enabled, "BATT_FAILSAFE", BATTERY_FAILSAFE),
// @Param: VOLT_DIVIDER
// @DisplayName: Voltage Divider
// @Description: TODO

8
ArduCopter/config.h
View File

@ -303,9 +303,6 @@
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef BATTERY_EVENT
# define BATTERY_EVENT DISABLED
#endif
#ifndef LOW_VOLTAGE
# define LOW_VOLTAGE 9.6
#endif
@ -323,7 +320,10 @@
# define HIGH_DISCHARGE 1760
#endif
// Battery failsafe
#ifndef BATTERY_FAILSAFE
# define BATTERY_FAILSAFE DISABLED
#endif

47
ArduCopter/events.pde
View File

@ -6,6 +6,11 @@
*/
static void failsafe_on_event()
{
// if motors are not armed there is nothing to do
if( !motors.armed() ) {
return;
}
// This is how to handle a failsafe.
switch(control_mode) {
case STABILIZE:
@ -27,6 +32,7 @@ static void failsafe_on_event()
case AUTO:
// throttle_fs_action is 1 do RTL, everything else means continue with the mission
if (g.throttle_fs_action == THROTTLE_FAILSAFE_ACTION_ALWAYS_RTL) {
// To-Do: check distance from home before initiating RTL
set_mode(RTL);
}
// if throttle_fs_action is 2 (i.e. THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION) no need to do anything
@ -35,6 +41,7 @@ static void failsafe_on_event()
if(ap.home_is_set == true) {
// same as above ^
// do_rtl sets the altitude to the current altitude by default
// To-Do: check distance from home before initiating RTL
set_mode(RTL);
}else{
// We have no GPS so we will crash land in alt hold mode
@ -58,19 +65,39 @@ static void failsafe_off_event()
static void low_battery_event(void)
{
static uint32_t last_low_battery_message;
uint32_t tnow = millis();
if (((uint32_t)(tnow - last_low_battery_message)) > 5000) {
// only send this message at 5s intervals at most or we may
// flood the link
gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!"));
last_low_battery_message = tnow;
// warn the ground station
gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!"));
// failsafe check
if (g.battery_fs_enabled && !ap.low_battery && motors.armed()) {
switch(control_mode) {
case STABILIZE:
case ACRO:
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
init_disarm_motors();
}else{
set_mode(LAND);
}
break;
case AUTO:
// To-Do: check distance to home before initiating RTL?
set_mode(RTL);
break;
default:
set_mode(LAND);
break;
}
}
// set the low battery flag
set_low_battery(true);
// if we are in Auto mode, come home
if(control_mode >= AUTO)
set_mode(RTL);
#if COPTER_LEDS == ENABLED
if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only
piezo_on();
}
#endif // COPTER_LEDS
}

35
ArduCopter/sensors.pde
View File

@ -68,8 +68,12 @@ static void init_optflow()
#endif
}
// read_battery - check battery voltage and current and invoke failsafe if necessary
// called at 10hz
#define BATTERY_FS_COUNTER 100 // 100 iterations at 10hz is 10 seconds
static void read_battery(void)
{
static uint8_t low_battery_counter = 0;
if(g.battery_monitoring == 0) {
battery_voltage1 = 0;
@ -88,28 +92,15 @@ static void read_battery(void)
current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours)
}
#if BATTERY_EVENT == ENABLED
//if(battery_voltage < g.low_voltage)
// low_battery_event();
if((battery_voltage1 < g.low_voltage) || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity)) {
low_battery_event();
#if COPTER_LEDS == ENABLED
if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only
if (battery_voltage1 > 1) {
piezo_on();
}else{
piezo_off();
}
// check for low voltage or current if the low voltage check hasn't already been triggered
if (!ap.low_battery && ( battery_voltage1 < g.low_voltage || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity))) {
low_battery_counter++;
if( low_battery_counter >= BATTERY_FS_COUNTER ) {
low_battery_counter = BATTERY_FS_COUNTER; // ensure counter does not overflow
low_battery_event();
}
}else if ( bitRead(g.copter_leds_mode, 3) ) {
piezo_off();
#endif // COPTER_LEDS
}else{
// reset low_battery_counter in case it was a temporary voltage dip
low_battery_counter = 0;
}
#endif //BATTERY_EVENT
}
//v: 10.9453, a: 17.4023, mah: 8.2