void Rover::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 == 1 && have_position) {
set_home_to_current_location(false);
} else {
set_home(cmd.content.location, false);
}
}
// set_home_to_current_location_and_lock - set home to current location and lock so it cannot be moved
bool Copter::set_home_to_current_location_and_lock()
{
if (set_home_to_current_location()) {
set_home_state(HOME_SET_AND_LOCKED);
return true;
}
return false;
}
void Copter::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if(cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
set_home_to_current_location();
} else {
if (!far_from_EKF_origin(cmd.content.location)) {
set_home(cmd.content.location);
}
}
}
// checks if we should update ahrs home position from the EKF's position
void Rover::update_home_from_EKF()
{
// exit immediately if home already set
if (home_is_set != HOME_UNSET) {
return;
}
// move home to current ekf location (this will set home_state to HOME_SET)
set_home_to_current_location(false);
}
void Sub::do_set_home(const AP_Mission::Mission_Command& cmd)
{
if (cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) {
if (!set_home_to_current_location(false)) {
// silently ignore this failure
}
} else {
if (!far_from_EKF_origin(cmd.content.location)) {
if (!set_home(cmd.content.location, false)) {
// silently ignore this failure
}
}
}
}
// checks if we should update ahrs/RTL home position from the EKF
void Copter::update_home_from_EKF()
{
// exit immediately if home already set
if (ap.home_state != HOME_UNSET) {
return;
}
// special logic if home is set in-flight
if (motors.armed()) {
set_home_to_current_location_inflight();
} else {
// move home to current ekf location (this will set home_state to HOME_SET)
set_home_to_current_location();
}
}
// read at 10 hz
// set this to your trainer switch
void Rover::read_trim_switch()
{
switch ((enum ch7_option)g.ch7_option.get()) {
case CH7_DO_NOTHING:
break;
case CH7_SAVE_WP:
if (channel_learn->get_radio_in() > CH_7_PWM_TRIGGER) {
// switch is engaged
ch7_flag = true;
} else { // switch is disengaged
if (ch7_flag) {
ch7_flag = false;
if (control_mode == &mode_manual) {
hal.console->printf("Erasing waypoints\n");
// if SW7 is ON in MANUAL = Erase the Flight Plan
mission.clear();
if (channel_steer->get_control_in() > 3000) {
// if roll is full right store the current location as home
set_home_to_current_location(false);
}
} else if (control_mode == &mode_learning || control_mode == &mode_steering) {
// if SW7 is ON in LEARNING = record the Wp
// create new mission command
AP_Mission::Mission_Command cmd = {};
// set new waypoint to current location
cmd.content.location = current_loc;
// make the new command to a waypoint
cmd.id = MAV_CMD_NAV_WAYPOINT;
// save command
if (mission.add_cmd(cmd)) {
hal.console->printf("Learning waypoint %u", static_cast<uint32_t>(mission.num_commands()));
}
} else if (control_mode == &mode_auto) {
// if SW7 is ON in AUTO = set to RTL
set_mode(mode_rtl, MODE_REASON_TX_COMMAND);
break;
}
}
}
break;
}
}
// 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;
// run pre-arm-checks and display failures
if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
// 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 update_notify a few times to ensure the message gets out
for (uint8_t i=0; i<=10; i++) {
update_notify();
}
#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 (ap.home_state == HOME_UNSET) {
// 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);
} else if (ap.home_state == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location();
}
calc_distance_and_bearing();
// 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
// short delay to allow reading of rc inputs
delay(30);
// 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);
// reenable failsafe
failsafe_enable();
// perf monitor ignores delay due to arming
perf_ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
// return success
return true;
}
// read ch7 aux switch
void Rover::read_aux_switch()
{
// do not consume input during rc or throttle failsafe
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) || (failsafe.bits & FAILSAFE_EVENT_RC)) {
return;
}
// get ch7's current position
aux_switch_pos aux_ch7_pos = read_aux_switch_pos();
// return if no change to switch position
if (aux_ch7_pos == aux_ch7) {
return;
}
aux_ch7 = aux_ch7_pos;
switch ((enum ch7_option)g.ch7_option.get()) {
case CH7_DO_NOTHING:
break;
case CH7_SAVE_WP:
if (aux_ch7 == AUX_SWITCH_HIGH) {
// do nothing if in AUTO mode
if (control_mode == &mode_auto) {
return;
}
// if disarmed clear mission and set home to current location
if (!arming.is_armed()) {
mission.clear();
set_home_to_current_location(false);
return;
}
// record the waypoint if not in auto mode
if (control_mode != &mode_auto) {
// create new mission command
AP_Mission::Mission_Command cmd = {};
// set new waypoint to current location
cmd.content.location = current_loc;
// make the new command to a waypoint
cmd.id = MAV_CMD_NAV_WAYPOINT;
// save command
if (mission.add_cmd(cmd)) {
hal.console->printf("Added waypoint %u", static_cast<uint32_t>(mission.num_commands()));
}
}
}
break;
// learn cruise speed and throttle
case CH7_LEARN_CRUISE:
if (aux_ch7 == AUX_SWITCH_HIGH) {
cruise_learn_start();
} else if (aux_ch7 == AUX_SWITCH_LOW) {
cruise_learn_complete();
}
break;
}
}
// 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;
}
// 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)
{
// arming marker
// Flag used to track if we have armed the motors the first time.
// This is used to decide if we should run the ground_start routine
// which calibrates the IMU
static bool did_ground_start = false;
static bool in_arm_motors = false;
// exit immediately if already in this function
if (in_arm_motors) {
return false;
}
in_arm_motors = true;
// run pre-arm-checks and display failures
if(!pre_arm_checks(true) || !arm_checks(true, arming_from_gcs)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
// 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 update_notify a few times to ensure the message gets out
for (uint8_t i=0; i<=10; i++) {
update_notify();
}
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
#endif
// Remember Orientation
// --------------------
init_simple_bearing();
initial_armed_bearing = ahrs.yaw_sensor;
if (ap.home_state == HOME_UNSET) {
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
ahrs.get_NavEKF().resetHeightDatum();
Log_Write_Event(DATA_EKF_ALT_RESET);
} else if (ap.home_state == HOME_SET_NOT_LOCKED) {
// Reset home position if it has already been set before (but not locked)
set_home_to_current_location();
}
calc_distance_and_bearing();
if(did_ground_start == false) {
startup_ground(true);
// final check that gyros calibrated successfully
if (((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) && !ins.gyro_calibrated_ok_all()) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Gyro calibration failed"));
AP_Notify::flags.armed = false;
failsafe_enable();
in_arm_motors = false;
return false;
}
did_ground_start = true;
}
// check if we are using motor interlock control on an aux switch
set_using_interlock(check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK));
// if we are using motor interlock switch and it's enabled, fail to arm
if (ap.using_interlock && motors.get_interlock()){
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Interlock Enabled"));
AP_Notify::flags.armed = false;
return false;
}
// if we are not using Emergency Stop switch option, force Estop false to ensure motors
// can run normally
if (!check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
set_motor_emergency_stop(false);
// if we are using motor Estop switch, it must not be in Estop position
} else if (check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP) && ap.motor_emergency_stop){
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Motor Emergency Stopped"));
AP_Notify::flags.armed = false;
return false;
}
// 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
// short delay to allow reading of rc inputs
delay(30);
// 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);
// reenable failsafe
failsafe_enable();
// perf monitor ignores delay due to arming
perf_ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
// return success
return true;
}
// 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 Sub::init_arm_motors(AP_Arming::Method method)
{
static bool in_arm_motors = false;
// exit immediately if already in this function
if (in_arm_motors) {
return false;
}
in_arm_motors = true;
if (!arming.pre_arm_checks(true)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
}
// let logger know that we're armed (it may open logs e.g.)
AP::logger().set_vehicle_armed(true);
// disable cpu failsafe because initialising everything takes a while
mainloop_failsafe_disable();
// 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 CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
#endif
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)
// Always use absolute altitude for ROV
// ahrs.resetHeightDatum();
// Log_Write_Event(DATA_EKF_ALT_RESET);
} else if (ahrs.home_is_set() && !ahrs.home_is_locked()) {
// Reset home position if it has already been set before (but not locked)
if (!set_home_to_current_location(false)) {
// ignore this failure
}
}
// enable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(true);
hal.util->set_soft_armed(true);
// enable output to motors
enable_motor_output();
// finally actually arm the motors
motors.armed(true);
Log_Write_Event(DATA_ARMED);
// log flight mode in case it was changed while vehicle was disarmed
logger.Write_Mode(control_mode, control_mode_reason);
// reenable failsafe
mainloop_failsafe_enable();
// perf monitor ignores delay due to arming
scheduler.perf_info.ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
// return success
return true;
}
