2016-08-17 01:28:16 -03:00
|
|
|
#include "AP_Arming.h"
|
|
|
|
#include "Rover.h"
|
|
|
|
|
2017-06-30 15:48:35 -03:00
|
|
|
// perform pre_arm_rc_checks checks
|
2019-07-23 21:13:31 -03:00
|
|
|
bool AP_Arming_Rover::rc_calibration_checks(const bool display_failure)
|
2017-06-30 15:48:35 -03:00
|
|
|
{
|
|
|
|
// set rc-checks to success if RC checks are disabled
|
|
|
|
if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_RC)) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
const RC_Channel *channels[] = {
|
2020-05-23 17:51:49 -03:00
|
|
|
rover.channel_steer,
|
|
|
|
rover.channel_throttle
|
2017-06-30 15:48:35 -03:00
|
|
|
};
|
|
|
|
const char *channel_names[] = {"Steer", "Throttle"};
|
|
|
|
|
|
|
|
for (uint8_t i= 0 ; i < ARRAY_SIZE(channels); i++) {
|
|
|
|
const RC_Channel *channel = channels[i];
|
|
|
|
const char *channel_name = channel_names[i];
|
|
|
|
// check if radio has been calibrated
|
|
|
|
if (channel->get_radio_min() > 1300) {
|
2017-08-09 08:11:13 -03:00
|
|
|
check_failed(ARMING_CHECK_RC, display_failure, "%s radio min too high", channel_name);
|
2017-06-30 15:48:35 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (channel->get_radio_max() < 1700) {
|
2017-08-09 08:11:13 -03:00
|
|
|
check_failed(ARMING_CHECK_RC, display_failure, "%s radio max too low", channel_name);
|
2017-06-30 15:48:35 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
}
|
2019-07-23 21:13:31 -03:00
|
|
|
return AP_Arming::rc_calibration_checks(display_failure);
|
2017-06-30 15:48:35 -03:00
|
|
|
}
|
2017-11-29 02:30:37 -04:00
|
|
|
|
|
|
|
// performs pre_arm gps related checks and returns true if passed
|
|
|
|
bool AP_Arming_Rover::gps_checks(bool display_failure)
|
|
|
|
{
|
2019-06-02 01:05:32 -03:00
|
|
|
if (!rover.control_mode->requires_position() && !rover.control_mode->requires_velocity()) {
|
|
|
|
// we don't care!
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2019-04-01 00:41:26 -03:00
|
|
|
const AP_AHRS &ahrs = AP::ahrs();
|
|
|
|
|
|
|
|
// always check if inertial nav has started and is ready
|
2020-08-11 02:04:30 -03:00
|
|
|
char failure_msg[50] = {};
|
|
|
|
if (!ahrs.pre_arm_check(failure_msg, sizeof(failure_msg))) {
|
|
|
|
check_failed(display_failure, "AHRS: %s", failure_msg);
|
2019-04-01 00:41:26 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2018-11-01 04:04:58 -03:00
|
|
|
// check for ekf failsafe
|
|
|
|
if (rover.failsafe.ekf) {
|
2019-10-04 18:51:02 -03:00
|
|
|
check_failed(display_failure, "EKF failsafe");
|
2018-11-01 04:04:58 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2018-12-18 06:28:20 -04:00
|
|
|
// ensure position esetimate is ok
|
|
|
|
if (!rover.ekf_position_ok()) {
|
2020-08-11 02:04:30 -03:00
|
|
|
// vehicle level position estimate checks
|
|
|
|
check_failed(display_failure, "Need Position Estimate");
|
2018-12-18 06:28:20 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2017-11-29 02:30:37 -04:00
|
|
|
// call parent gps checks
|
|
|
|
return AP_Arming::gps_checks(display_failure);
|
|
|
|
}
|
2017-12-09 02:54:47 -04:00
|
|
|
|
|
|
|
bool AP_Arming_Rover::pre_arm_checks(bool report)
|
|
|
|
{
|
2019-05-05 22:15:32 -03:00
|
|
|
//are arming checks disabled?
|
2019-10-04 18:51:02 -03:00
|
|
|
if (checks_to_perform == 0) {
|
2019-05-05 22:15:32 -03:00
|
|
|
return true;
|
|
|
|
}
|
2019-01-25 14:58:50 -04:00
|
|
|
if (SRV_Channels::get_emergency_stop()) {
|
2019-10-04 18:51:02 -03:00
|
|
|
check_failed(report, "Motors Emergency Stopped");
|
2019-01-25 14:58:50 -04:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2020-07-28 08:35:22 -03:00
|
|
|
if (rover.g2.sailboat.sail_enabled() && !rover.g2.windvane.enabled()) {
|
|
|
|
check_failed(report, "Sailing enabled with no WindVane");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
2017-08-16 07:02:56 -03:00
|
|
|
return (AP_Arming::pre_arm_checks(report)
|
2020-07-29 06:00:19 -03:00
|
|
|
& rover.g2.motors.pre_arm_check(report)
|
|
|
|
& fence_checks(report)
|
|
|
|
& oa_check(report)
|
|
|
|
& parameter_checks(report)
|
|
|
|
& mode_checks(report));
|
2017-08-16 07:02:56 -03:00
|
|
|
}
|
|
|
|
|
2019-05-05 22:15:32 -03:00
|
|
|
bool AP_Arming_Rover::arm_checks(AP_Arming::Method method)
|
|
|
|
{
|
|
|
|
//are arming checks disabled?
|
2019-10-04 18:51:02 -03:00
|
|
|
if (checks_to_perform == 0) {
|
2019-05-05 22:15:32 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
return AP_Arming::arm_checks(method);
|
|
|
|
}
|
|
|
|
|
2019-05-29 20:37:14 -03:00
|
|
|
void AP_Arming_Rover::update_soft_armed()
|
|
|
|
{
|
|
|
|
hal.util->set_soft_armed(is_armed() &&
|
|
|
|
hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED);
|
|
|
|
AP::logger().set_vehicle_armed(hal.util->get_soft_armed());
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
arm motors
|
|
|
|
*/
|
|
|
|
bool AP_Arming_Rover::arm(AP_Arming::Method method, const bool do_arming_checks)
|
|
|
|
{
|
|
|
|
if (!AP_Arming::arm(method, do_arming_checks)) {
|
|
|
|
AP_Notify::events.arming_failed = true;
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Set the SmartRTL home location. If activated, SmartRTL will ultimately try to land at this point
|
|
|
|
rover.g2.smart_rtl.set_home(true);
|
|
|
|
|
|
|
|
// initialize simple mode heading
|
|
|
|
rover.mode_simple.init_heading();
|
|
|
|
|
|
|
|
// save home heading for use in sail vehicles
|
|
|
|
rover.g2.windvane.record_home_heading();
|
|
|
|
|
2019-10-04 19:41:57 -03:00
|
|
|
update_soft_armed();
|
2019-05-29 20:37:14 -03:00
|
|
|
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Throttle armed");
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
disarm motors
|
|
|
|
*/
|
2020-02-21 09:09:57 -04:00
|
|
|
bool AP_Arming_Rover::disarm(const AP_Arming::Method method)
|
2019-05-29 20:37:14 -03:00
|
|
|
{
|
2020-02-21 09:09:57 -04:00
|
|
|
if (!AP_Arming::disarm(method)) {
|
2019-05-29 20:37:14 -03:00
|
|
|
return false;
|
|
|
|
}
|
|
|
|
if (rover.control_mode != &rover.mode_auto) {
|
|
|
|
// reset the mission on disarm if we are not in auto
|
|
|
|
rover.mode_auto.mission.reset();
|
|
|
|
}
|
|
|
|
|
2019-10-04 19:41:57 -03:00
|
|
|
update_soft_armed();
|
2019-05-29 20:37:14 -03:00
|
|
|
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Throttle disarmed");
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
2019-05-10 02:59:52 -03:00
|
|
|
|
|
|
|
// check object avoidance has initialised correctly
|
|
|
|
bool AP_Arming_Rover::oa_check(bool report)
|
|
|
|
{
|
|
|
|
char failure_msg[50];
|
|
|
|
if (rover.g2.oa.pre_arm_check(failure_msg, ARRAY_SIZE(failure_msg))) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// display failure
|
|
|
|
if (strlen(failure_msg) == 0) {
|
2019-10-04 18:51:02 -03:00
|
|
|
check_failed(report, "Check Object Avoidance");
|
2019-05-10 02:59:52 -03:00
|
|
|
} else {
|
2019-10-04 18:51:02 -03:00
|
|
|
check_failed(report, "%s", failure_msg);
|
2019-05-10 02:59:52 -03:00
|
|
|
}
|
|
|
|
return false;
|
|
|
|
}
|
2019-09-17 22:29:25 -03:00
|
|
|
|
|
|
|
// perform parameter checks
|
|
|
|
bool AP_Arming_Rover::parameter_checks(bool report)
|
|
|
|
{
|
|
|
|
// success if parameter checks are disabled
|
|
|
|
if ((checks_to_perform != ARMING_CHECK_ALL) && !(checks_to_perform & ARMING_CHECK_PARAMETERS)) {
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
// check waypoint speed is positive
|
|
|
|
if (!is_positive(rover.g2.wp_nav.get_default_speed())) {
|
|
|
|
check_failed(ARMING_CHECK_PARAMETERS, report, "WP_SPEED too low");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2020-02-06 06:07:10 -04:00
|
|
|
// check if arming allowed from this mode
|
|
|
|
bool AP_Arming_Rover::mode_checks(bool report)
|
2020-05-23 17:51:49 -03:00
|
|
|
{
|
2020-02-06 06:07:10 -04:00
|
|
|
//display failure if arming in this mode is not allowed
|
|
|
|
if (!rover.control_mode->allows_arming()) {
|
|
|
|
check_failed(report, "Mode not armable");
|
|
|
|
return false;
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|