// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
#if ADVANCED_FAILSAFE == ENABLED
// this is to allow the failsafe module to deliberately crash
// the vehicle. Only used in extreme circumstances to meet the
// OBC rules
if (g2.afs.should_crash_vehicle()) {
g2.afs.terminate_vehicle();
return;
}
#endif
// Update arming delay state
if (ap.in_arming_delay && (!motors.armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
ap.in_arming_delay = false;
}
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
bool interlock = motors.armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop;
if (!motors.get_interlock() && interlock) {
motors.set_interlock(true);
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
} else if (motors.get_interlock() && !interlock) {
motors.set_interlock(false);
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
}
// send output signals to motors
motors.output();
}
}
// Run landing gear controller at 10Hz
void Copter::landinggear_update()
{
// exit immediately if no landing gear output has been enabled
if (!SRV_Channels::function_assigned(SRV_Channel::k_landing_gear_control)) {
return;
}
// last status (deployed or retracted) used to check for changes, initialised to startup state of landing gear
static bool last_deploy_status = landinggear.deployed();
// if we are doing an automatic landing procedure, force the landing gear to deploy.
// To-Do: should we pause the auto-land procedure to give time for gear to come down?
if (control_mode == LAND ||
(control_mode == RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) ||
(control_mode == AUTO && auto_mode == Auto_Land) ||
(control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) {
landinggear.set_position(AP_LandingGear::LandingGear_Deploy_And_Keep_Deployed);
}
// send event message to datalog if status has changed
if (landinggear.deployed() != last_deploy_status) {
if (landinggear.deployed()) {
Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED);
} else {
Log_Write_Event(DATA_LANDING_GEAR_RETRACTED);
}
}
last_deploy_status = landinggear.deployed();
}
// Run landing gear controller at 10Hz
void Sub::landinggear_update(){
// If landing gear control is active, run update function.
if (check_if_auxsw_mode_used(AUXSW_LANDING_GEAR)){
// last status (deployed or retracted) used to check for changes
static bool last_deploy_status;
// if we are doing an automatic landing procedure, force the landing gear to deploy.
// To-Do: should we pause the auto-land procedure to give time for gear to come down?
if (control_mode == LAND ||
(control_mode==RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) ||
(control_mode == AUTO && auto_mode == Auto_Land) ||
(control_mode == AUTO && auto_mode == Auto_RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent))) {
landinggear.force_deploy(true);
}
landinggear.update();
// send event message to datalog if status has changed
if (landinggear.deployed() != last_deploy_status){
if (landinggear.deployed()) {
Log_Write_Event(DATA_LANDING_GEAR_DEPLOYED);
} else {
Log_Write_Event(DATA_LANDING_GEAR_RETRACTED);
}
}
last_deploy_status = landinggear.deployed();
}
}
void Copter::set_motor_emergency_stop(bool b)
{
if(ap.motor_emergency_stop != b) {
ap.motor_emergency_stop = b;
}
// Log new status
if (ap.motor_emergency_stop){
Log_Write_Event(DATA_MOTORS_EMERGENCY_STOPPED);
} else {
Log_Write_Event(DATA_MOTORS_EMERGENCY_STOP_CLEARED);
}
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
#if ADVANCED_FAILSAFE == ENABLED
// this is to allow the failsafe module to deliberately crash
// the vehicle. Only used in extreme circumstances to meet the
// OBC rules
if (g2.afs.should_crash_vehicle()) {
g2.afs.terminate_vehicle();
if (!g2.afs.terminating_vehicle_via_landing()) {
return;
}
// landing must continue to run the motors output
}
#endif
// Update arming delay state
if (ap.in_arming_delay && (!motors->armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
ap.in_arming_delay = false;
}
// output any servo channels
SRV_Channels::calc_pwm();
// cork now, so that all channel outputs happen at once
SRV_Channels::cork();
// update output on any aux channels, for manual passthru
SRV_Channels::output_ch_all();
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !SRV_Channels::get_emergency_stop();
if (!motors->get_interlock() && interlock) {
motors->set_interlock(true);
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
} else if (motors->get_interlock() && !interlock) {
motors->set_interlock(false);
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
}
// send output signals to motors
motors->output();
}
// push all channels
SRV_Channels::push();
}
void Sub::set_surfaced(bool at_surface) {
if(ap.at_surface == at_surface) { // do nothing if state unchanged
return;
}
ap.at_surface = at_surface;
if(!ap.at_surface) {
surface_detector_count = 0;
Log_Write_Event(DATA_SURFACED);
gcs_send_text(MAV_SEVERITY_INFO, "Off Surface");
} else {
Log_Write_Event(DATA_NOT_SURFACED);
gcs_send_text(MAV_SEVERITY_INFO, "Surfaced");
}
}
void Sub::set_bottomed(bool at_bottom) {
if(ap.at_bottom == at_bottom) { // do nothing if state unchanged
return;
}
ap.at_bottom = at_bottom;
if(!ap.at_bottom) {
bottom_detector_count = 0;
Log_Write_Event(DATA_BOTTOMED);
gcs_send_text(MAV_SEVERITY_INFO, "Off Bottom");
} else {
Log_Write_Event(DATA_NOT_BOTTOMED);
gcs_send_text(MAV_SEVERITY_INFO, "Bottomed");
}
}
void Sub::set_bottomed(bool at_bottom)
{
if (ap.at_bottom == at_bottom) { // do nothing if state unchanged
return;
}
ap.at_bottom = at_bottom;
bottom_detector_count = 0;
if (ap.at_bottom) {
Log_Write_Event(DATA_BOTTOMED);
} else {
Log_Write_Event(DATA_NOT_BOTTOMED);
}
}
// do_gripper - control gripper
void Copter::do_gripper(const AP_Mission::Mission_Command& cmd)
{
// Note: we ignore the gripper num parameter because we only support one gripper
switch (cmd.content.gripper.action) {
case GRIPPER_ACTION_RELEASE:
g2.gripper.release();
Log_Write_Event(DATA_GRIPPER_RELEASE);
break;
case GRIPPER_ACTION_GRAB:
g2.gripper.grab();
Log_Write_Event(DATA_GRIPPER_GRAB);
break;
default:
// do nothing
break;
}
}
// flip_init - initialise flip controller
bool Copter::flip_init(bool ignore_checks)
{
#if FRAME_CONFIG == HELI_FRAME
// Flip mode not available for helis as it is untested.
return false;
#endif
// only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
if (control_mode != ACRO && control_mode != STABILIZE && control_mode != ALT_HOLD) {
return false;
}
// if in acro or stabilize ensure throttle is above zero
if (ap.throttle_zero && (control_mode == ACRO || control_mode == STABILIZE)) {
return false;
}
// ensure roll input is less than 40deg
if (abs(channel_roll->control_in) >= 4000) {
return false;
}
// only allow flip when flying
if (!motors.armed() || ap.land_complete) {
return false;
}
// capture original flight mode so that we can return to it after completion
flip_orig_control_mode = control_mode;
// initialise state
flip_state = Flip_Start;
flip_start_time = millis();
flip_roll_dir = flip_pitch_dir = 0;
// choose direction based on pilot's roll and pitch sticks
if (channel_pitch->control_in > 300) {
flip_pitch_dir = FLIP_PITCH_BACK;
}else if(channel_pitch->control_in < -300) {
flip_pitch_dir = FLIP_PITCH_FORWARD;
}else if (channel_roll->control_in >= 0) {
flip_roll_dir = FLIP_ROLL_RIGHT;
}else{
flip_roll_dir = FLIP_ROLL_LEFT;
}
// log start of flip
Log_Write_Event(DATA_FLIP_START);
// capture current attitude which will be used during the Flip_Recovery stage
flip_orig_attitude.x = constrain_float(ahrs.roll_sensor, -aparm.angle_max, aparm.angle_max);
flip_orig_attitude.y = constrain_float(ahrs.pitch_sensor, -aparm.angle_max, aparm.angle_max);
flip_orig_attitude.z = ahrs.yaw_sensor;
return true;
}
// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object
void Copter::heli_update_rotor_speed_targets()
{
static bool rotor_runup_complete_last = false;
// get rotor control method
uint8_t rsc_control_mode = motors.get_rsc_mode();
rsc_control_deglitched = rotor_speed_deglitch_filter.apply(g.rc_8.control_in);
switch (rsc_control_mode) {
case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH:
// pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom
if (rsc_control_deglitched > 10) {
motors.set_interlock(true);
motors.set_desired_rotor_speed(rsc_control_deglitched);
} else {
motors.set_interlock(false);
motors.set_desired_rotor_speed(0);
}
break;
case ROTOR_CONTROL_MODE_SPEED_SETPOINT:
case ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT:
case ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT:
// pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode
// other than being used to create a crude estimate of rotor speed
if (rsc_control_deglitched > 0) {
motors.set_interlock(true);
motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
}else{
motors.set_interlock(false);
motors.set_desired_rotor_speed(0);
}
break;
}
// when rotor_runup_complete changes to true, log event
if (!rotor_runup_complete_last && motors.rotor_runup_complete()){
Log_Write_Event(DATA_ROTOR_RUNUP_COMPLETE);
} else if (rotor_runup_complete_last && !motors.rotor_runup_complete()){
Log_Write_Event(DATA_ROTOR_SPEED_BELOW_CRITICAL);
}
rotor_runup_complete_last = motors.rotor_runup_complete();
}
// land_nogps_run - runs the land controller
// pilot controls roll and pitch angles
// should be called at 100hz or more
void Copter::ModeLand::nogps_run()
{
float target_roll = 0.0f, target_pitch = 0.0f;
float target_yaw_rate = 0;
// process pilot inputs
if (!copter.failsafe.radio) {
if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){
Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT);
// exit land if throttle is high
copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE);
}
if (g.land_repositioning) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// get pilot desired lean angles
get_pilot_desired_lean_angles(target_roll, target_pitch, copter.aparm.angle_max, attitude_control->get_althold_lean_angle_max());
}
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
}
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
attitude_control->set_throttle_out(0,false,g.throttle_filt);
#else
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
// multicopters do not stabilize roll/pitch/yaw when disarmed
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
#endif
// disarm when the landing detector says we've landed
if (ap.land_complete) {
copter.init_disarm_motors();
}
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// pause before beginning land descent
if (land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
land_pause = false;
}
land_run_vertical_control(land_pause);
}
// save_trim - adds roll and pitch trims from the radio to ahrs
void Copter::save_trim()
{
// save roll and pitch trim
float roll_trim = ToRad((float)channel_roll->get_control_in()/100.0f);
float pitch_trim = ToRad((float)channel_pitch->get_control_in()/100.0f);
ahrs.add_trim(roll_trim, pitch_trim);
Log_Write_Event(DATA_SAVE_TRIM);
gcs_send_text(MAV_SEVERITY_INFO, "Trim saved");
}
// save_trim - adds roll and pitch trims from the radio to ahrs
void Copter::save_trim()
{
// save roll and pitch trim
float roll_trim = ToRad((float)channel_roll->control_in/100.0f);
float pitch_trim = ToRad((float)channel_pitch->control_in/100.0f);
ahrs.add_trim(roll_trim, pitch_trim);
Log_Write_Event(DATA_SAVE_TRIM);
gcs_send_text_P(MAV_SEVERITY_CRITICAL, PSTR("Trim saved"));
}
// ---------------------------------------------
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 Sub::set_surfaced(bool at_surface)
{
if (ap.at_surface == at_surface) { // do nothing if state unchanged
return;
}
ap.at_surface = at_surface;
surface_detector_count = 0;
if (ap.at_surface) {
Log_Write_Event(DATA_SURFACED);
} else {
Log_Write_Event(DATA_NOT_SURFACED);
}
}
// set_home - sets ahrs home (used for RTL) to specified location
// initialises inertial nav and compass on first call
// returns true if home location set successfully
bool Copter::set_home(const Location& loc, bool lock)
{
// check location is valid
if (loc.lat == 0 && loc.lng == 0) {
return false;
}
// check EKF origin has been set
Location ekf_origin;
if (!ahrs.get_origin(ekf_origin)) {
return false;
}
// check home is close to EKF origin
if (far_from_EKF_origin(loc)) {
return false;
}
const bool home_was_set = ahrs.home_is_set();
// set ahrs home (used for RTL)
ahrs.set_home(loc);
// init inav and compass declination
if (!home_was_set) {
// update navigation scalers. used to offset the shrinking longitude as we go towards the poles
scaleLongDown = longitude_scale(loc);
// record home is set
Log_Write_Event(DATA_SET_HOME);
#if MODE_AUTO_ENABLED == ENABLED
// log new home position which mission library will pull from ahrs
if (should_log(MASK_LOG_CMD)) {
AP_Mission::Mission_Command temp_cmd;
if (mission.read_cmd_from_storage(0, temp_cmd)) {
DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd);
}
}
#endif
}
// lock home position
if (lock) {
ahrs.lock_home();
}
// log ahrs home and ekf origin dataflash
ahrs.Log_Write_Home_And_Origin();
// send new home and ekf origin to GCS
gcs().send_home();
gcs().send_ekf_origin();
// return success
return true;
}
// flip_init - initialise flip controller
bool Copter::ModeFlip::init(bool ignore_checks)
{
// only allow flip from ACRO, Stabilize, AltHold or Drift flight modes
if (copter.control_mode != ACRO &&
copter.control_mode != STABILIZE &&
copter.control_mode != ALT_HOLD &&
copter.control_mode != FLOWHOLD) {
return false;
}
// if in acro or stabilize ensure throttle is above zero
if (ap.throttle_zero && (copter.control_mode == ACRO || copter.control_mode == STABILIZE)) {
return false;
}
// ensure roll input is less than 40deg
if (abs(channel_roll->get_control_in()) >= 4000) {
return false;
}
// only allow flip when flying
if (!motors->armed() || ap.land_complete) {
return false;
}
// capture original flight mode so that we can return to it after completion
orig_control_mode = copter.control_mode;
// initialise state
_state = FlipState::Start;
start_time_ms = millis();
roll_dir = pitch_dir = 0;
// choose direction based on pilot's roll and pitch sticks
if (channel_pitch->get_control_in() > 300) {
pitch_dir = FLIP_PITCH_BACK;
} else if (channel_pitch->get_control_in() < -300) {
pitch_dir = FLIP_PITCH_FORWARD;
} else if (channel_roll->get_control_in() >= 0) {
roll_dir = FLIP_ROLL_RIGHT;
} else {
roll_dir = FLIP_ROLL_LEFT;
}
// log start of flip
Log_Write_Event(DATA_FLIP_START);
// capture current attitude which will be used during the FlipState::Recovery stage
const float angle_max = copter.aparm.angle_max;
orig_attitude.x = constrain_float(ahrs.roll_sensor, -angle_max, angle_max);
orig_attitude.y = constrain_float(ahrs.pitch_sensor, -angle_max, angle_max);
orig_attitude.z = ahrs.yaw_sensor;
return true;
}
// check for ekf yaw reset and adjust target heading
void Copter::check_ekf_yaw_reset()
{
float yaw_angle_change_rad = 0.0f;
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
if (new_ekfYawReset_ms != ekfYawReset_ms) {
attitude_control.shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);
ekfYawReset_ms = new_ekfYawReset_ms;
Log_Write_Event(DATA_EKF_YAW_RESET);
}
}
// do_parachute - configure or release parachute
void Copter::do_parachute(const AP_Mission::Mission_Command& cmd)
{
switch (cmd.p1) {
case PARACHUTE_DISABLE:
parachute.enabled(false);
Log_Write_Event(DATA_PARACHUTE_DISABLED);
break;
case PARACHUTE_ENABLE:
parachute.enabled(true);
Log_Write_Event(DATA_PARACHUTE_ENABLED);
break;
case PARACHUTE_RELEASE:
parachute_release();
break;
default:
// do nothing
break;
}
}
// ---------------------------------------------
void Copter::set_auto_armed(bool b)
{
// if no change, exit immediately
if( ap.auto_armed == b )
return;
ap.auto_armed = b;
if(b){
Log_Write_Event(DATA_AUTO_ARMED);
}
}
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
void Copter::parachute_release()
{
// send message to gcs and dataflash
gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released");
Log_Write_Event(DATA_PARACHUTE_RELEASED);
// disarm motors
init_disarm_motors();
// release parachute
parachute.release();
}
// control winch based on mission command
void Copter::ModeAuto::do_winch(const AP_Mission::Mission_Command& cmd)
{
// Note: we ignore the gripper num parameter because we only support one gripper
switch (cmd.content.winch.action) {
case WINCH_RELAXED:
g2.winch.relax();
Log_Write_Event(DATA_WINCH_RELAXED);
break;
case WINCH_RELATIVE_LENGTH_CONTROL:
g2.winch.release_length(cmd.content.winch.release_length, cmd.content.winch.release_rate);
Log_Write_Event(DATA_WINCH_LENGTH_CONTROL);
break;
case WINCH_RATE_CONTROL:
g2.winch.set_desired_rate(cmd.content.winch.release_rate);
Log_Write_Event(DATA_WINCH_RATE_CONTROL);
break;
default:
// do nothing
break;
}
}
// ---------------------------------------------
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_disarm_motors - disarm motors
void Copter::init_disarm_motors()
{
// return immediately if we are already disarmed
if (!motors.armed()) {
return;
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "Disarming motors");
#endif
// save compass offsets learned by the EKF if enabled
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
Vector3f magOffsets;
if (ahrs.getMagOffsets(i, magOffsets)) {
compass.set_and_save_offsets(i, magOffsets);
}
}
}
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
autotune_save_tuning_gains();
#endif
// we are not in the air
set_land_complete(true);
set_land_complete_maybe(true);
// log disarm to the dataflash
Log_Write_Event(DATA_DISARMED);
// send disarm command to motors
motors.armed(false);
// reset the mission
mission.reset();
// suspend logging
if (!DataFlash.log_while_disarmed()) {
DataFlash.EnableWrites(false);
}
DataFlash_Class::instance()->set_vehicle_armed(false);
// disable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(false);
hal.util->set_soft_armed(false);
ap.in_arming_delay = false;
}
void Sub::set_land_complete(bool b)
{
// if no change, exit immediately
if( ap.land_complete == b )
return;
land_detector_count = 0;
if(b){
Log_Write_Event(DATA_LAND_COMPLETE);
} else {
Log_Write_Event(DATA_NOT_LANDED);
}
ap.land_complete = b;
// trigger disarm-on-land if configured
bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode);
if (ap.land_complete && motors.armed() && disarm_on_land_configured && mode_disarms_on_land) {
init_disarm_motors();
}
}
// set_home_state - update home state
void Copter::set_home_state(enum HomeState new_home_state)
{
// if no change, exit immediately
if (ap.home_state == new_home_state)
return;
// update state
ap.home_state = new_home_state;
// log if home has been set
if (new_home_state == HOME_SET_NOT_LOCKED || new_home_state == HOME_SET_AND_LOCKED) {
Log_Write_Event(DATA_SET_HOME);
}
}
// checks if we should update ahrs/RTL home position from GPS
void Copter::set_system_time_from_GPS()
{
// exit immediately if system time already set
if (ap.system_time_set) {
return;
}
// if we have a 3d lock and valid location
if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
// set system clock for log timestamps
hal.util->set_system_clock(gps.time_epoch_usec());
ap.system_time_set = true;
Log_Write_Event(DATA_SYSTEM_TIME_SET);
}
}
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
void Sub::parachute_release()
{
// send message to gcs and dataflash
gcs_send_text(MAV_SEVERITY_INFO,"Parachute: Released");
Log_Write_Event(DATA_PARACHUTE_RELEASED);
// disarm motors
init_disarm_motors();
// release parachute
parachute.release();
// deploy landing gear
landinggear.set_cmd_mode(LandingGear_Deploy);
}
// init_disarm_motors - disarm motors
void Copter::init_disarm_motors()
{
// return immediately if we are already disarmed
if (!motors.armed()) {
return;
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text(MAV_SEVERITY_INFO, "DISARMING MOTORS");
#endif
// save compass offsets learned by the EKF
Vector3f magOffsets;
if (ahrs.use_compass() && ahrs.getMagOffsets(magOffsets)) {
compass.set_and_save_offsets(compass.get_primary(), magOffsets);
}
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
autotune_save_tuning_gains();
#endif
// we are not in the air
set_land_complete(true);
set_land_complete_maybe(true);
// log disarm to the dataflash
Log_Write_Event(DATA_DISARMED);
// send disarm command to motors
motors.armed(false);
// reset the mission
mission.reset();
// suspend logging
if (!(g.log_bitmask & MASK_LOG_WHEN_DISARMED)) {
DataFlash.EnableWrites(false);
}
// disable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(false);
hal.util->set_soft_armed(false);
}
