2012-11-10 01:43:43 -04:00
|
|
|
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
|
|
|
|
|
|
|
|
void set_home_is_set(bool b)
|
|
|
|
{
|
2013-01-12 11:13:10 -04:00
|
|
|
// if no change, exit immediately
|
|
|
|
if( ap.home_is_set == b )
|
|
|
|
return;
|
2012-11-10 01:43:43 -04:00
|
|
|
|
2013-01-12 11:13:10 -04:00
|
|
|
ap.home_is_set = b;
|
|
|
|
if(b) {
|
|
|
|
Log_Write_Event(DATA_SET_HOME);
|
|
|
|
}
|
2012-11-10 01:43:43 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// ---------------------------------------------
|
|
|
|
void set_auto_armed(bool b)
|
|
|
|
{
|
2013-01-12 11:13:10 -04:00
|
|
|
// if no change, exit immediately
|
|
|
|
if( ap.auto_armed == b )
|
|
|
|
return;
|
|
|
|
|
|
|
|
ap.auto_armed = b;
|
|
|
|
if(b){
|
|
|
|
Log_Write_Event(DATA_AUTO_ARMED);
|
|
|
|
}
|
2012-11-10 01:43:43 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// ---------------------------------------------
|
2013-09-11 02:36:38 -03:00
|
|
|
void set_simple_mode(uint8_t b)
|
2012-11-10 01:43:43 -04:00
|
|
|
{
|
2013-09-11 02:36:38 -03:00
|
|
|
if(ap.simple_mode != b){
|
|
|
|
if(b == 0){
|
2013-07-27 20:28:00 -03:00
|
|
|
Log_Write_Event(DATA_SET_SIMPLE_OFF);
|
2013-09-11 02:36:38 -03:00
|
|
|
}else if(b == 1){
|
2013-01-12 11:13:10 -04:00
|
|
|
Log_Write_Event(DATA_SET_SIMPLE_ON);
|
|
|
|
}else{
|
2013-10-05 06:25:03 -03:00
|
|
|
// initialise super simple heading
|
|
|
|
update_super_simple_bearing(true);
|
2013-07-27 20:28:00 -03:00
|
|
|
Log_Write_Event(DATA_SET_SUPERSIMPLE_ON);
|
2013-01-12 11:13:10 -04:00
|
|
|
}
|
2013-09-11 02:36:38 -03:00
|
|
|
ap.simple_mode = b;
|
2013-01-12 11:13:10 -04:00
|
|
|
}
|
2012-11-10 01:43:43 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// ---------------------------------------------
|
2013-09-11 02:36:38 -03:00
|
|
|
static void set_failsafe_radio(bool b)
|
2012-11-10 01:43:43 -04:00
|
|
|
{
|
|
|
|
// only act on changes
|
|
|
|
// -------------------
|
2013-09-26 05:54:33 -03:00
|
|
|
if(failsafe.radio != b) {
|
2012-11-10 01:43:43 -04:00
|
|
|
|
|
|
|
// store the value so we don't trip the gate twice
|
|
|
|
// -----------------------------------------------
|
2013-09-26 05:54:33 -03:00
|
|
|
failsafe.radio = b;
|
2012-11-10 01:43:43 -04:00
|
|
|
|
2013-09-26 05:54:33 -03:00
|
|
|
if (failsafe.radio == false) {
|
2012-11-10 01:43:43 -04:00
|
|
|
// We've regained radio contact
|
|
|
|
// ----------------------------
|
2013-03-16 05:14:21 -03:00
|
|
|
failsafe_radio_off_event();
|
2012-11-10 01:43:43 -04:00
|
|
|
}else{
|
|
|
|
// We've lost radio contact
|
|
|
|
// ------------------------
|
2013-03-16 05:14:21 -03:00
|
|
|
failsafe_radio_on_event();
|
2012-11-10 01:43:43 -04:00
|
|
|
}
|
2013-09-11 02:36:38 -03:00
|
|
|
|
|
|
|
// update AP_Notify
|
|
|
|
AP_Notify::flags.failsafe_radio = b;
|
2012-11-10 01:43:43 -04:00
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// ---------------------------------------------
|
2013-10-13 01:52:52 -03:00
|
|
|
void set_failsafe_battery(bool b)
|
2012-11-10 01:43:43 -04:00
|
|
|
{
|
2013-10-13 01:52:52 -03:00
|
|
|
failsafe.battery = b;
|
2013-09-11 02:36:38 -03:00
|
|
|
AP_Notify::flags.failsafe_battery = b;
|
2012-11-10 01:43:43 -04:00
|
|
|
}
|
|
|
|
|
2013-03-16 05:27:46 -03:00
|
|
|
|
|
|
|
// ---------------------------------------------
|
2013-09-11 02:36:38 -03:00
|
|
|
static void set_failsafe_gps(bool b)
|
2013-03-16 05:27:46 -03:00
|
|
|
{
|
2013-09-26 05:54:33 -03:00
|
|
|
failsafe.gps = b;
|
2013-09-22 10:20:47 -03:00
|
|
|
|
|
|
|
// update AP_Notify
|
|
|
|
AP_Notify::flags.failsafe_gps = b;
|
2013-03-16 05:27:46 -03:00
|
|
|
}
|
|
|
|
|
2013-04-29 09:30:22 -03:00
|
|
|
// ---------------------------------------------
|
2013-09-11 02:36:38 -03:00
|
|
|
static void set_failsafe_gcs(bool b)
|
2013-04-29 09:30:22 -03:00
|
|
|
{
|
2013-09-26 05:54:33 -03:00
|
|
|
failsafe.gcs = b;
|
2013-04-29 09:30:22 -03:00
|
|
|
}
|
|
|
|
|
2012-11-10 01:43:43 -04:00
|
|
|
// ---------------------------------------------
|
|
|
|
void set_takeoff_complete(bool b)
|
|
|
|
{
|
2013-01-12 11:13:10 -04:00
|
|
|
// if no change, exit immediately
|
|
|
|
if( ap.takeoff_complete == b )
|
|
|
|
return;
|
|
|
|
|
|
|
|
if(b){
|
|
|
|
Log_Write_Event(DATA_TAKEOFF);
|
|
|
|
}
|
|
|
|
ap.takeoff_complete = b;
|
2012-11-10 01:43:43 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// ---------------------------------------------
|
|
|
|
void set_land_complete(bool b)
|
|
|
|
{
|
2013-01-12 11:13:10 -04:00
|
|
|
// if no change, exit immediately
|
|
|
|
if( ap.land_complete == b )
|
|
|
|
return;
|
|
|
|
|
|
|
|
if(b){
|
|
|
|
Log_Write_Event(DATA_LAND_COMPLETE);
|
2013-07-26 02:15:27 -03:00
|
|
|
}else{
|
|
|
|
Log_Write_Event(DATA_NOT_LANDED);
|
2013-01-12 11:13:10 -04:00
|
|
|
}
|
|
|
|
ap.land_complete = b;
|
2012-11-10 01:43:43 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
// ---------------------------------------------
|
|
|
|
|
2013-08-14 00:07:35 -03:00
|
|
|
void set_pre_arm_check(bool b)
|
|
|
|
{
|
|
|
|
if(ap.pre_arm_check != b) {
|
|
|
|
ap.pre_arm_check = b;
|
|
|
|
AP_Notify::flags.pre_arm_check = b;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-11-15 04:12:31 -04:00
|
|
|
void set_pre_arm_rc_check(bool b)
|
|
|
|
{
|
|
|
|
if(ap.pre_arm_rc_check != b) {
|
|
|
|
ap.pre_arm_rc_check = b;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|