Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: move lost copter check to separate function

Browse Source 
Also run from scheduler instead of from arm/disarm stick checks
Run at 10hz instead of 1hz
soundalarm_counter to uint8
Disable if equivalent aux switch is setup
Remove SEVERITY_LOW message because the GCSs never print these anyway
This commit is contained in:
Randy Mackay 2015-04-28 23:06:39 +09:00
parent 5bee93cba3
commit 276b2dcf5e
2 changed files with 30 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/ArduCopter.pde
View File

@ -742,6 +742,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
{ ekf_check, 40, 2 },
{ crash_check, 40, 2 },
{ landinggear_update, 40, 1 },
{ lost_vehicle_check, 40, 2 },
{ gcs_check_input, 1, 550 },
{ gcs_send_heartbeat, 400, 150 },
{ gcs_send_deferred, 8, 720 },

50
ArduCopter/motors.pde
View File

@ -4,7 +4,7 @@
#define DISARM_DELAY 20 // called at 10hz so 2 seconds
#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds
#define AUTO_DISARMING_DELAY 15 // called at 1hz so 15 seconds
#define NOTIFY_LOCATION_DELAY 1 // called at 1hz so 1 seconds
#define LOST_VEHICLE_DELAY 10 // called at 10hz so 1 second
static uint8_t auto_disarming_counter;
@ -72,26 +72,6 @@ static void arm_motors_check()
// called at 1hz
static void auto_disarm_check()
{
static int16_t soundalarm_counter;
// ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
if (!g.rc_3.control_in > 0 && !motors.armed() && g.rc_1.control_in > 4000 && g.rc_2.control_in > 4000) {
if (soundalarm_counter >= NOTIFY_LOCATION_DELAY) {
if (AP_Notify::flags.vehicle_lost == false) {
AP_Notify::flags.vehicle_lost = true;
gcs_send_text_P(SEVERITY_HIGH,PSTR("Locate Copter Alarm!!"));
}
} else {
soundalarm_counter++;
}
}else{
soundalarm_counter = 0;
if (AP_Notify::flags.vehicle_lost == true) {
AP_Notify::flags.vehicle_lost = false;
gcs_send_text_P(SEVERITY_LOW,PSTR("Locate Copter Alarm Off"));
}
}
// exit immediately if we are already disarmed or throttle is not zero
if (!motors.armed() || !ap.throttle_zero) {
auto_disarming_counter = 0;
@ -749,3 +729,31 @@ static void motors_output()
motors.output();
}
}
// check for pilot stick input to trigger lost vehicle alarm
static void lost_vehicle_check()
{
static uint8_t soundalarm_counter;
// disable if aux switch is setup to vehicle alarm as the two could interfere
if (check_if_auxsw_mode_used(AUXSW_LOST_COPTER_SOUND)) {
return;
}
// ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
if (ap.throttle_zero && !motors.armed() && (g.rc_1.control_in > 4000) && (g.rc_2.control_in > 4000)) {
if (soundalarm_counter >= LOST_VEHICLE_DELAY) {
if (AP_Notify::flags.vehicle_lost == false) {
AP_Notify::flags.vehicle_lost = true;
gcs_send_text_P(SEVERITY_HIGH,PSTR("Locate Copter Alarm!"));
}
} else {
soundalarm_counter++;
}
} else {
soundalarm_counter = 0;
if (AP_Notify::flags.vehicle_lost == true) {
AP_Notify::flags.vehicle_lost = false;
}
}
}