Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: move arming-related functionality into parent classes 

Logging moves up
arming via mavlink moves up
arming via switch moves up

arming switch may be forced off using BRD parameter
This commit is contained in:
Peter Barker 2019-05-04 10:14:56 +10:00 committed by Randy Mackay
parent 400aa53654
commit 8566a17a1d
6 changed files with 48 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduSub/AP_Arming_Sub.h
View File

@ -14,6 +14,9 @@ public:
bool rc_calibration_checks(bool display_failure) override;
bool pre_arm_checks(bool display_failure) override;
bool disarm() override;
bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
protected:
bool ins_checks(bool display_failure) override;
};

18
ArduSub/GCS_Mavlink.cpp
View File

@ -511,22 +511,6 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_long_packet(const mavlink_command_lon
}
return MAV_RESULT_FAILED;
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f)) {
// attempt to arm and return success or failure
if (sub.init_arm_motors(AP_Arming::Method::MAVLINK)) {
return MAV_RESULT_ACCEPTED;
}
} else if (is_zero(packet.param1)) {
// force disarming by setting param2 = 21196 is deprecated
// see COMMAND_LONG DO_FLIGHTTERMINATION
sub.init_disarm_motors();
return MAV_RESULT_ACCEPTED;
} else {
return MAV_RESULT_UNSUPPORTED;
}
return MAV_RESULT_FAILED;
case MAV_CMD_DO_MOTOR_TEST:
// param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
// param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
@ -850,7 +834,7 @@ void Sub::mavlink_delay_cb()
MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_long_t &packet) {
if (packet.param1 > 0.5f) {
sub.init_disarm_motors();
sub.arming.disarm();
return MAV_RESULT_ACCEPTED;
}
return MAV_RESULT_FAILED;

2
ArduSub/Sub.h
View File

@ -588,8 +588,6 @@ private:
void update_surface_and_bottom_detector();
void set_surfaced(bool at_surface);
void set_bottomed(bool at_bottom);
bool init_arm_motors(AP_Arming::Method method);
void init_disarm_motors();
void motors_output();
Vector3f pv_location_to_vector(const Location& loc);
float pv_alt_above_origin(float alt_above_home_cm);

18
ArduSub/failsafe.cpp
View File

@ -92,7 +92,7 @@ void Sub::failsafe_sensors_check()
// This should always succeed
if (!set_mode(MANUAL, MODE_REASON_BAD_DEPTH)) {
// We should never get here
init_disarm_motors();
arming.disarm();
}
}
}
@ -146,7 +146,7 @@ void Sub::failsafe_ekf_check()
}
if (g.fs_ekf_action == FS_EKF_ACTION_DISARM) {
init_disarm_motors();
arming.disarm();
}
}
@ -160,7 +160,7 @@ void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE);
break;
case Failsafe_Action_Disarm:
init_disarm_motors();
arming.disarm();
break;
case Failsafe_Action_Warn:
case Failsafe_Action_None:
@ -194,7 +194,7 @@ void Sub::failsafe_pilot_input_check()
set_neutral_controls();
if(g.failsafe_pilot_input == FS_PILOT_INPUT_DISARM) {
init_disarm_motors();
arming.disarm();
}
#endif
}
@ -345,14 +345,14 @@ void Sub::failsafe_gcs_check()
// handle failsafe action
if (g.failsafe_gcs == FS_GCS_DISARM) {
init_disarm_motors();
arming.disarm();
} else if (g.failsafe_gcs == FS_GCS_HOLD && motors.armed()) {
if (!set_mode(ALT_HOLD, MODE_REASON_GCS_FAILSAFE)) {
init_disarm_motors();
arming.disarm();
}
} else if (g.failsafe_gcs == FS_GCS_SURFACE && motors.armed()) {
if (!set_mode(SURFACE, MODE_REASON_GCS_FAILSAFE)) {
init_disarm_motors();
arming.disarm();
}
}
}
@ -411,7 +411,7 @@ void Sub::failsafe_crash_check()
// disarm motors
if (g.fs_crash_check == FS_CRASH_DISARM) {
init_disarm_motors();
arming.disarm();
}
}
@ -490,6 +490,6 @@ void Sub::failsafe_terrain_act()
case FS_TERRAIN_DISARM:
default:
init_disarm_motors();
arming.disarm();
}
}

8
ArduSub/joystick.cpp
View File

@ -128,16 +128,16 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
switch (get_button(button)->function(shift)) {
case JSButton::button_function_t::k_arm_toggle:
if (motors.armed()) {
init_disarm_motors();
arming.disarm();
} else {
init_arm_motors(AP_Arming::Method::MAVLINK);
arming.arm(AP_Arming::Method::MAVLINK);
}
break;
case JSButton::button_function_t::k_arm:
init_arm_motors(AP_Arming::Method::MAVLINK);
arming.arm(AP_Arming::Method::MAVLINK);
break;
case JSButton::button_function_t::k_disarm:
init_disarm_motors();
arming.disarm();
break;
case JSButton::button_function_t::k_mode_manual:

55
ArduSub/motors.cpp
View File

@ -6,9 +6,7 @@ void Sub::enable_motor_output()
motors.output_min();
}
// 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)
bool AP_Arming_Sub::arm(AP_Arming::Method method, bool do_arming_checks)
{
static bool in_arm_motors = false;
@ -19,7 +17,7 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
in_arm_motors = true;
if (!arming.pre_arm_checks(true)) {
if (!AP_Arming::arm(method, do_arming_checks)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
@ -29,20 +27,22 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
AP::logger().set_vehicle_armed(true);
// disable cpu failsafe because initialising everything takes a while
mainloop_failsafe_disable();
sub.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();
AP::notify().update();
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs().send_text(MAV_SEVERITY_INFO, "Arming motors");
#endif
initial_armed_bearing = ahrs.yaw_sensor;
AP_AHRS &ahrs = AP::ahrs();
sub.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)
@ -52,7 +52,7 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
// 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)) {
if (!sub.set_home_to_current_location(false)) {
// ignore this failure
}
}
@ -62,21 +62,21 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
hal.util->set_soft_armed(true);
// enable output to motors
enable_motor_output();
sub.enable_motor_output();
// finally actually arm the motors
motors.armed(true);
sub.motors.armed(true);
Log_Write_Event(DATA_ARMED);
AP::logger().Write_Event(DATA_ARMED);
// log flight mode in case it was changed while vehicle was disarmed
logger.Write_Mode(control_mode, control_mode_reason);
AP::logger().Write_Mode(sub.control_mode, sub.control_mode_reason);
// reenable failsafe
mainloop_failsafe_enable();
sub.mainloop_failsafe_enable();
// perf monitor ignores delay due to arming
scheduler.perf_info.ignore_this_loop();
AP::scheduler().perf_info.ignore_this_loop();
// flag exiting this function
in_arm_motors = false;
@ -85,35 +85,40 @@ bool Sub::init_arm_motors(AP_Arming::Method method)
return true;
}
// init_disarm_motors - disarm motors
void Sub::init_disarm_motors()
bool AP_Arming_Sub::disarm()
{
// return immediately if we are already disarmed
if (!motors.armed()) {
return;
if (!sub.motors.armed()) {
return false;
}
if (!AP_Arming::disarm()) {
return false;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors");
#endif
AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
// save compass offsets learned by the EKF if enabled
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
if (ahrs.use_compass() && AP::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);
AP::compass().set_and_save_offsets(i, magOffsets);
}
}
}
Log_Write_Event(DATA_DISARMED);
AP::logger().Write_Event(DATA_DISARMED);
// send disarm command to motors
motors.armed(false);
sub.motors.armed(false);
// reset the mission
mission.reset();
sub.mission.reset();
AP::logger().set_vehicle_armed(false);
@ -122,7 +127,9 @@ void Sub::init_disarm_motors()
hal.util->set_soft_armed(false);
// clear input holds
clear_input_hold();
sub.clear_input_hold();
return true;
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos