// calc_home_distance_and_bearing - calculate distance and bearing to home for reporting and autopilot decisions
void Copter::calc_home_distance_and_bearing()
{
// calculate home distance and bearing
if (position_ok()) {
Vector3f home = pv_location_to_vector(ahrs.get_home());
Vector3f curr = inertial_nav.get_position();
home_distance = pv_get_horizontal_distance_cm(curr, home);
home_bearing = pv_get_bearing_cd(curr,home);
// update super simple bearing (if required) because it relies on home_bearing
update_super_simple_bearing(false);
}
}
// ---------------------------------------------
void Copter::set_simple_mode(uint8_t b)
{
if(ap.simple_mode != b) {
if(b == 0) {
Log_Write_Event(DATA_SET_SIMPLE_OFF);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode off");
} else if(b == 1) {
Log_Write_Event(DATA_SET_SIMPLE_ON);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode on");
} else {
// initialise super simple heading
update_super_simple_bearing(true);
Log_Write_Event(DATA_SET_SUPERSIMPLE_ON);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
}
ap.simple_mode = b;
}
}
// ---------------------------------------------
void Copter::set_simple_mode(uint8_t b)
{
if(ap.simple_mode != b){
switch (b) {
case 0:
Log_Write_Event(DATA_SET_SIMPLE_OFF);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode off");
break;
case 1:
Log_Write_Event(DATA_SET_SIMPLE_ON);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SIMPLE mode on");
break;
default:
// initialise super simple heading
update_super_simple_bearing(true);
Log_Write_Event(DATA_SET_SUPERSIMPLE_ON);
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "SUPERSIMPLE mode on");
break;
}
ap.simple_mode = b;
}
}
// init_arm_motors - performs arming process including initialisation of barometer and gyros
// returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure
bool Copter::init_arm_motors(bool arming_from_gcs)
{
static bool in_arm_motors = false;
// exit immediately if already in this function
if (in_arm_motors) {
return false;
}
in_arm_motors = true;
// return true if already armed
if (motors->armed()) {
in_arm_motors = false;
return true;
}
// run pre-arm-checks and display failures
if (!arming.all_checks_passing(arming_from_gcs)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
// let dataflash know that we're armed (it may open logs e.g.)
DataFlash_Class::instance()->set_vehicle_armed(true);
// disable cpu failsafe because initialising everything takes a while
failsafe_disable();
// reset battery failsafe
set_failsafe_battery(false);
// notify that arming will occur (we do this early to give plenty of warning)
AP_Notify::flags.armed = true;
// call notify update a few times to ensure the message gets out
for (uint8_t i=0; i<=10; i++) {
notify.update();
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
#endif
// Remember Orientation
// --------------------
init_simple_bearing();
initial_armed_bearing = ahrs.yaw_sensor;
if (!ahrs.home_is_set()) {
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
ahrs.resetHeightDatum();
Log_Write_Event(DATA_EKF_ALT_RESET);
// we have reset height, so arming height is zero
arming_altitude_m = 0;
} else if (ahrs.home_status() == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location(false);
// remember the height when we armed
arming_altitude_m = inertial_nav.get_altitude() * 0.01;
}
update_super_simple_bearing(false);
// Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
#if MODE_SMARTRTL_ENABLED == ENABLED
g2.smart_rtl.set_home(position_ok());
#endif
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);
#if SPRAYER == ENABLED
// turn off sprayer's test if on
sprayer.test_pump(false);
#endif
// enable output to motors
enable_motor_output();
// finally actually arm the motors
motors->armed(true);
// log arming to dataflash
Log_Write_Event(DATA_ARMED);
// log flight mode in case it was changed while vehicle was disarmed
DataFlash.Log_Write_Mode(control_mode, control_mode_reason);
// reenable failsafe
failsafe_enable();
// perf monitor ignores delay due to arming
scheduler.perf_info.ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
// Log time stamp of arming event
arm_time_ms = millis();
// Start the arming delay
ap.in_arming_delay = true;
// return success
return true;
}
// run_nav_updates - top level call for the autopilot
// ensures calculations such as "distance to waypoint" are calculated before autopilot makes decisions
// To-Do - rename and move this function to make it's purpose more clear
void Copter::run_nav_updates(void)
{
update_super_simple_bearing(false);
flightmode->update_navigation();
}
