mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-09 09:28:31 -04:00
82e51aec82
moved ground start to first arming added ground start flag moved throttle_integrator to 50hz loop CAMERA_STABILIZER deprecated - now always on renamed current logging bit mask to match APM added MA filter to PID - D term Adjusted PIDs based on continued testing and new PID filter added MASK_LOG_SET_DEFAULTS to match APM moved some stuff out of ground start into system start where it belonged Added slower Yaw gains for DCM when the copter is in the air changed camera output to be none scaled PWM fixed bug where ground_temperature was unfiltered shortened Baro startup time fixed issue with Nav_WP integrator not being reset RTL no longer yaws towards home Circle mode for flying a 10m circle around the point where it was engaged. - Not tested at all! Consider Circle mode as alpha. git-svn-id: https://arducopter.googlecode.com/svn/trunk@2966 f9c3cf11-9bcb-44bc-f272-b75c42450872
101 lines
2.0 KiB
Plaintext
101 lines
2.0 KiB
Plaintext
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
/*
|
|
This event will be called when the failsafe changes
|
|
boolean failsafe reflects the current state
|
|
*/
|
|
static void failsafe_on_event()
|
|
{
|
|
// This is how to handle a failsafe.
|
|
switch(control_mode)
|
|
{
|
|
case AUTO:
|
|
if (g.throttle_fs_action == 1) {
|
|
set_mode(RTL);
|
|
}
|
|
// 2 = Stay in AUTO and ignore failsafe
|
|
|
|
default:
|
|
// not ready to enable yet w/o more testing
|
|
//set_mode(RTL);
|
|
break;
|
|
}
|
|
}
|
|
|
|
static void failsafe_off_event()
|
|
{
|
|
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();
|
|
|
|
// Reset control integrators
|
|
// ---------------------
|
|
reset_I();
|
|
|
|
}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)
|
|
{
|
|
gcs.send_text_P(SEVERITY_HIGH,PSTR("Low Battery!"));
|
|
low_batt = true;
|
|
|
|
// if we are in Auto mode, come home
|
|
if(control_mode >= AUTO)
|
|
set_mode(RTL);
|
|
}
|
|
|
|
|
|
static void update_events() // Used for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
|
|
{
|
|
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();
|
|
}
|
|
}
|
|
}
|
|
|
|
static void relay_on()
|
|
{
|
|
PORTL |= B00000100;
|
|
}
|
|
|
|
static void relay_off()
|
|
{
|
|
PORTL &= ~B00000100;
|
|
}
|
|
|
|
static void relay_toggle()
|
|
{
|
|
PORTL ^= B00000100;
|
|
}
|
|
|