diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0aa571345e..e8db3ac271 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -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 }, diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 9d896369fd..a3d50b5811 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -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; + } + } +}