ardupilot/ArduCopter/AP_State.cpp

94 lines
2.6 KiB
C++
Raw Normal View History

#include "Copter.h"
2012-11-10 01:43:43 -04:00
// ---------------------------------------------
void Copter::set_auto_armed(bool b)
2012-11-10 01:43:43 -04:00
{
// if no change, exit immediately
if( ap.auto_armed == b )
return;
ap.auto_armed = b;
if(b){
AP::logger().Write_Event(LogEvent::AUTO_ARMED);
}
2012-11-10 01:43:43 -04:00
}
// ---------------------------------------------
/**
* Set Simple mode
*
* @param [in] b 0:false or disabled, 1:true or SIMPLE, 2:SUPERSIMPLE
*/
2020-06-15 10:05:09 -03:00
void Copter::set_simple_mode(SimpleMode b)
2012-11-10 01:43:43 -04:00
{
2020-06-15 10:05:09 -03:00
if (simple_mode != b) {
switch (b) {
2020-06-15 10:05:09 -03:00
case SimpleMode::NONE:
AP::logger().Write_Event(LogEvent::SET_SIMPLE_OFF);
gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode off");
break;
2020-06-15 10:05:09 -03:00
case SimpleMode::SIMPLE:
AP::logger().Write_Event(LogEvent::SET_SIMPLE_ON);
gcs().send_text(MAV_SEVERITY_INFO, "SIMPLE mode on");
break;
2020-06-15 10:05:09 -03:00
case SimpleMode::SUPERSIMPLE:
// initialise super simple heading
update_super_simple_bearing(true);
AP::logger().Write_Event(LogEvent::SET_SUPERSIMPLE_ON);
gcs().send_text(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
break;
}
2020-06-15 10:05:09 -03:00
simple_mode = b;
}
2012-11-10 01:43:43 -04:00
}
// ---------------------------------------------
void Copter::set_failsafe_radio(bool b)
2012-11-10 01:43:43 -04:00
{
// only act on changes
// -------------------
if(failsafe.radio != b) {
2012-11-10 01:43:43 -04:00
// store the value so we don't trip the gate twice
// -----------------------------------------------
failsafe.radio = b;
2012-11-10 01:43:43 -04:00
if (failsafe.radio == false) {
2012-11-10 01:43:43 -04:00
// We've regained radio contact
// ----------------------------
failsafe_radio_off_event();
2012-11-10 01:43:43 -04:00
}else{
// We've lost radio contact
// ------------------------
failsafe_radio_on_event();
2012-11-10 01:43:43 -04:00
}
// update AP_Notify
AP_Notify::flags.failsafe_radio = b;
2012-11-10 01:43:43 -04:00
}
}
2013-04-29 09:30:22 -03:00
// ---------------------------------------------
void Copter::set_failsafe_gcs(bool b)
2013-04-29 09:30:22 -03:00
{
failsafe.gcs = b;
// update AP_Notify
AP_Notify::flags.failsafe_gcs = b;
2013-04-29 09:30:22 -03:00
}
2012-11-10 01:43:43 -04:00
// ---------------------------------------------
void Copter::update_using_interlock()
{
#if FRAME_CONFIG == HELI_FRAME
// helicopters are always using motor interlock
ap.using_interlock = true;
#else
// check if we are using motor interlock control on an aux switch or are in throw mode
// which uses the interlock to stop motors while the copter is being thrown
ap.using_interlock = rc().find_channel_for_option(RC_Channel::AUX_FUNC::MOTOR_INTERLOCK) != nullptr;
#endif
}