ardupilot/ArduCopter/events.pde

126 lines
3.9 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-11-13 10:43:54 -04:00
// if motors are not armed there is nothing to do
if( !motors.armed() ) {
return;
}
2012-08-16 21:50:02 -03:00
// This is how to handle a failsafe.
switch(control_mode) {
case STABILIZE:
case ACRO:
// if throttle is zero disarm motors
if (g.rc_3.control_in == 0) {
init_disarm_motors();
}else if(ap.home_is_set == true) {
set_mode(RTL);
}else{
// We have no GPS so we will land
set_mode(LAND);
}
break;
case AUTO:
// failsafe_throttle is 1 do RTL, 2 means continue with the mission
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
2012-11-13 10:43:54 -04:00
// To-Do: check distance from home before initiating RTL
set_mode(RTL);
}
// if failsafe_throttle is 2 (i.e. FS_THR_ENABLED_CONTINUE_MISSION) no need to do anything
break;
default:
if(ap.home_is_set == true) {
// same as above ^
// do_rtl sets the altitude to the current altitude by default
2012-11-13 10:43:54 -04:00
// 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
// the low throttle will bring us down at the maximum descent speed
// To-Do: make LAND a throttle mode so it can operate without GPS
set_mode(ALT_HOLD);
set_new_altitude(0);
}
break;
2012-08-16 21:50:02 -03:00
}
}
// failsafe_off_event - respond to radio contact being regained
// we must be in AUTO, LAND or RTL modes
// or Stabilize or ACRO mode but with motors disarmed
static void failsafe_off_event()
{
// no need to do anything
// user can now override roll, pitch, yaw and throttle and even use flight mode switch to restore previous flight mode
}
static void low_battery_event(void)
{
2012-11-13 10:43:54 -04:00
// warn the ground station
gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!"));
// failsafe check
if (g.failsafe_battery_enabled && !ap.low_battery && motors.armed()) {
2012-11-13 10:43:54 -04:00
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;
}
}
2012-11-13 10:43:54 -04:00
// set the low battery flag
2012-11-10 01:55:51 -04:00
set_low_battery(true);
2012-11-13 10:43:54 -04:00
#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
}
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) {
2012-12-12 21:25:09 -04:00
hal.rcout->write(event_id, event_value); // send to Servos
2012-08-16 21:50:02 -03:00
} else {
2012-12-12 21:25:09 -04:00
hal.rcout->write(event_id, event_undo_value);
2012-08-16 21:50:02 -03:00
}
}
if (event_id == RELAY_TOGGLE) {
relay.toggle();
}
}
}