ardupilot/ArduCopter/events.pde

109 lines
3.0 KiB
Plaintext
Raw Normal View History

// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
2012-08-16 21:50:02 -03:00
* This event will be called when the failsafe changes
* boolean failsafe reflects the current state
*/
static void failsafe_on_event()
{
2012-08-16 21:50:02 -03:00
// This is how to handle a failsafe.
switch(control_mode)
{
case AUTO:
if (g.throttle_fs_action == 1) {
// do_rtl sets the altitude to the current altitude by default
set_mode(RTL);
// We add an additional 10m to the current altitude
//next_WP.alt += 1000;
set_new_altitude(next_WP.alt + 1000);
}
//if (g.throttle_fs_action == 2)
// Stay in AUTO and ignore failsafe
break;
default:
2012-11-10 01:55:51 -04:00
if(ap.home_is_set) {
2012-08-16 21:50:02 -03:00
// same as above ^
// do_rtl sets the altitude to the current altitude by default
set_mode(RTL);
// We add an additional 10m to the current altitude
//next_WP.alt += 1000;
set_new_altitude(next_WP.alt + 1000);
}else{
// We have no GPS so we must land
set_mode(LAND);
}
break;
}
}
static void failsafe_off_event()
{
2012-08-16 21:50:02 -03:00
// If we are in AUTO, no need to do anything
if(control_mode == AUTO)
return;
if (g.throttle_fs_action == 2) {
// We're back in radio contact
// return to AP
// ---------------------------
// re-read the switch so we can return to our preferred mode
// --------------------------------------------------------
reset_control_switch();
}else if (g.throttle_fs_action == 1) {
// We're back in radio contact
// return to Home
// we should already be in RTL and throttle set to cruise
// ------------------------------------------------------
set_mode(RTL);
}
}
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;
}
2012-11-10 01:55:51 -04:00
set_low_battery(true);
2012-08-16 21:50:02 -03:00
// if we are in Auto mode, come home
if(control_mode >= AUTO)
set_mode(RTL);
}
2012-08-16 21:50:02 -03:00
static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
{
2012-08-16 21:50:02 -03:00
if(event_repeat == 0 || (millis() - event_timer) < event_delay)
return;
if (event_repeat > 0) {
event_repeat--;
}
if(event_repeat != 0) { // event_repeat = -1 means repeat forever
event_timer = millis();
if (event_id >= CH_5 && event_id <= CH_8) {
if(event_repeat%2) {
APM_RC.OutputCh(event_id, event_value); // send to Servos
} else {
APM_RC.OutputCh(event_id, event_undo_value);
}
}
if (event_id == RELAY_TOGGLE) {
relay.toggle();
}
}
}