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

Copter: use AP_Arming methods to arm and disarm vehicle 

Really just changing the namespace of init_arm_motors
This commit is contained in:
Peter Barker 2019-05-06 11:39:57 +10:00 committed by Randy Mackay
parent 61a2be1470
commit 6dce39cbe1
17 changed files with 96 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduCopter/AP_Arming.cpp
View File

@ -12,20 +12,19 @@ void AP_Arming_Copter::update(void)
pre_arm_display_counter = 0;
}
set_pre_arm_check(pre_arm_checks(display_fail));
pre_arm_checks(display_fail);
}
// performs pre-arm checks and arming checks
bool AP_Arming_Copter::all_checks_passing(AP_Arming::Method method)
bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
{
set_pre_arm_check(pre_arm_checks(true));
return copter.ap.pre_arm_check && arm_checks(method);
const bool passed = run_pre_arm_checks(display_failure);
set_pre_arm_check(passed);
return passed;
}
// perform pre-arm checks
// return true if the checks pass successfully
bool AP_Arming_Copter::pre_arm_checks(bool display_failure)
bool AP_Arming_Copter::run_pre_arm_checks(bool display_failure)
{
// exit immediately if already armed
if (copter.motors->armed()) {

8
ArduCopter/AP_Arming.h
View File

@ -20,10 +20,12 @@ public:
AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete;
void update(void);
bool all_checks_passing(AP_Arming::Method method);
bool rc_calibration_checks(bool display_failure) override;
bool disarm() override;
bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
protected:
bool pre_arm_checks(bool display_failure) override;
@ -48,4 +50,8 @@ protected:
private:
// actually contains the pre-arm checks. This is wrapped so that
// we can store away success/failure of the checks.
bool run_pre_arm_checks(bool display_failure);
};

2
ArduCopter/Copter.h
View File

@ -752,8 +752,6 @@ private:
// motors.cpp
void arm_motors_check();
void auto_disarm_check();
bool init_arm_motors(AP_Arming::Method method, bool do_arming_checks=true);
void init_disarm_motors();
void motors_output();
void lost_vehicle_check();

31
ArduCopter/GCS_Mavlink.cpp
View File

@ -606,6 +606,11 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t
return GCS_MAVLINK::handle_command_mount(packet);
}
bool GCS_MAVLINK_Copter::allow_disarm() const
{
return copter.ap.land_complete;
}
MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet)
{
switch(packet.command) {
@ -699,26 +704,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
return MAV_RESULT_FAILED;
#endif
case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f)) {
// attempt to arm and return success or failure
const bool do_arming_checks = !is_equal(packet.param2,magic_force_arm_value);
if (copter.init_arm_motors(AP_Arming::Method::MAVLINK, do_arming_checks)) {
return MAV_RESULT_ACCEPTED;
}
} else if (is_zero(packet.param1)) {
if (copter.ap.land_complete || is_equal(packet.param2,magic_force_disarm_value)) {
// force disarming by setting param2 = 21196 is deprecated
copter.init_disarm_motors();
return MAV_RESULT_ACCEPTED;
} else {
return MAV_RESULT_FAILED;
}
} else {
return MAV_RESULT_UNSUPPORTED;
}
return MAV_RESULT_FAILED;
#if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE:
// configure or release parachute
@ -819,7 +804,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
if (!copter.motors->armed()) {
// if disarmed, arm motors
copter.init_arm_motors(AP_Arming::Method::MAVLINK);
copter.arming.arm(AP_Arming::Method::MAVLINK);
} else if (copter.ap.land_complete) {
// if armed and landed, takeoff
if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
@ -841,7 +826,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
if (copter.motors->armed()) {
if (copter.ap.land_complete) {
// if landed, disarm motors
copter.init_disarm_motors();
copter.arming.disarm();
} else {
// assume that shots modes are all done in guided.
// NOTE: this may need to change if we add a non-guided shot mode
@ -1309,7 +1294,7 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_flight_termination(const mavlink_command_l
if (GCS_MAVLINK::handle_flight_termination(packet) != MAV_RESULT_ACCEPTED) {
#endif
if (packet.param1 > 0.5f) {
copter.init_disarm_motors();
copter.arming.disarm();
result = MAV_RESULT_ACCEPTED;
}
#if ADVANCED_FAILSAFE == ENABLED

2
ArduCopter/GCS_Mavlink.h
View File

@ -43,6 +43,8 @@ protected:
virtual MAV_VTOL_STATE vtol_state() const override { return MAV_VTOL_STATE_MC; };
virtual MAV_LANDED_STATE landed_state() const override;
bool allow_disarm() const override;
private:
void handleMessage(mavlink_message_t * msg) override;

4
ArduCopter/RC_Channel.cpp
View File

@ -420,7 +420,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
// arm or disarm the vehicle
switch (ch_flag) {
case HIGH:
copter.init_arm_motors(AP_Arming::Method::AUXSWITCH);
copter.arming.arm(AP_Arming::Method::AUXSWITCH);
// remember that we are using an arming switch, for use by set_throttle_zero_flag
copter.ap.armed_with_switch = true;
break;
@ -428,7 +428,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
// nothing
break;
case LOW:
copter.init_disarm_motors();
copter.arming.disarm();
break;
}
break;

2
ArduCopter/afs_copter.cpp
View File

@ -25,7 +25,7 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
copter.motors->output();
// disarm as well
copter.init_disarm_motors();
copter.arming.disarm();
// and set all aux channels
SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);

2
ArduCopter/avoidance_adsb.cpp
View File

@ -34,7 +34,7 @@ MAV_COLLISION_ACTION AP_Avoidance_Copter::handle_avoidance(const AP_Avoidance::O
// if landed and we will take some kind of action, just disarm
if ((actual_action > MAV_COLLISION_ACTION_REPORT) && copter.should_disarm_on_failsafe()) {
copter.init_disarm_motors();
copter.arming.disarm();
actual_action = MAV_COLLISION_ACTION_NONE;
} else {

4
ArduCopter/crash_check.cpp
View File

@ -53,7 +53,7 @@ void Copter::crash_check()
// send message to gcs
gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
// disarm motors
init_disarm_motors();
copter.arming.disarm();
}
}
@ -210,7 +210,7 @@ void Copter::parachute_check()
void Copter::parachute_release()
{
// disarm motors
init_disarm_motors();
arming.disarm();
// release parachute
parachute.release();

10
ArduCopter/events.cpp
View File

@ -12,7 +12,7 @@ void Copter::failsafe_radio_on_event()
}
if (should_disarm_on_failsafe()) {
init_disarm_motors();
arming.disarm();
} else {
if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
// continue mission
@ -55,7 +55,7 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
// failsafe check
if (should_disarm_on_failsafe()) {
init_disarm_motors();
arming.disarm();
} else {
switch ((Failsafe_Action)action) {
case Failsafe_Action_None:
@ -78,7 +78,7 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
snprintf(battery_type_str, 17, "%s battery", type_str);
g2.afs.gcs_terminate(true, battery_type_str);
#else
init_disarm_motors();
arming.disarm();
#endif
}
}
@ -122,7 +122,7 @@ void Copter::failsafe_gcs_check()
RC_Channels::clear_overrides();
if (should_disarm_on_failsafe()) {
init_disarm_motors();
arming.disarm();
} else {
if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
// continue mission
@ -190,7 +190,7 @@ void Copter::failsafe_terrain_on_event()
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
if (should_disarm_on_failsafe()) {
init_disarm_motors();
arming.disarm();
#if MODE_RTL_ENABLED == ENABLED
} else if (control_mode == RTL) {
mode_rtl.restart_without_terrain();

2
ArduCopter/fence.cpp
View File

@ -30,7 +30,7 @@ void Copter::fence_check()
// disarm immediately if we think we are on the ground or in a manual flight mode with zero throttle
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if (ap.land_complete || (flightmode->has_manual_throttle() && ap.throttle_zero && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0))){
init_disarm_motors();
arming.disarm();
} else {

2
ArduCopter/land_detector.cpp
View File

@ -117,7 +117,7 @@ void Copter::set_land_complete(bool b)
const bool mode_disarms_on_land = flightmode->allows_arming(false) && !flightmode->has_manual_throttle();
if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
init_disarm_motors();
arming.disarm();
}
}

2
ArduCopter/mode_auto.cpp
View File

@ -537,7 +537,7 @@ void Copter::ModeAuto::exit_mission()
}
} else {
// if we've landed it's safe to disarm
copter.init_disarm_motors();
copter.arming.disarm();
}
}

4
ArduCopter/mode_land.cpp
View File

@ -55,7 +55,7 @@ void Copter::ModeLand::gps_run()
{
// disarm when the landing detector says we've landed
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.init_disarm_motors();
copter.arming.disarm();
}
// Land State Machine Determination
@ -109,7 +109,7 @@ void Copter::ModeLand::nogps_run()
// disarm when the landing detector says we've landed
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.init_disarm_motors();
copter.arming.disarm();
}
// Land State Machine Determination

2
ArduCopter/mode_rtl.cpp
View File

@ -378,7 +378,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
// disarm when the landing detector says we've landed
if (disarm_on_land && ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
copter.init_disarm_motors();
copter.arming.disarm();
}
// if not armed set throttle to zero and exit immediately

92
ArduCopter/motors.cpp
View File

@ -45,7 +45,7 @@ void Copter::arm_motors_check()
// arm the motors and configure for flight
if (arming_counter == ARM_DELAY && !motors->armed()) {
// reset arming counter if arming fail
if (!init_arm_motors(AP_Arming::Method::RUDDER)) {
if (!arming.arm(AP_Arming::Method::RUDDER)) {
arming_counter = 0;
}
}
@ -71,7 +71,7 @@ void Copter::arm_motors_check()
// disarm the motors
if (arming_counter == DISARM_DELAY && motors->armed()) {
init_disarm_motors();
arming.disarm();
}
// Yaw is centered so reset arming counter
@ -124,14 +124,12 @@ void Copter::auto_disarm_check()
// disarm once timer expires
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
init_disarm_motors();
arming.disarm();
auto_disarm_begin = tnow_ms;
}
}
// 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(const AP_Arming::Method method, const bool do_arming_checks)
bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_checks)
{
static bool in_arm_motors = false;
@ -142,13 +140,12 @@ bool Copter::init_arm_motors(const AP_Arming::Method method, const bool do_armin
in_arm_motors = true;
// return true if already armed
if (motors->armed()) {
if (copter.motors->armed()) {
in_arm_motors = false;
return true;
}
// run pre-arm-checks and display failures
if (do_arming_checks && !arming.all_checks_passing(method)) {
if (!AP_Arming::arm(method, do_arming_checks)) {
AP_Notify::events.arming_failed = true;
in_arm_motors = false;
return false;
@ -158,13 +155,13 @@ bool Copter::init_arm_motors(const AP_Arming::Method method, const bool do_armin
AP::logger().set_vehicle_armed(true);
// disable cpu failsafe because initialising everything takes a while
failsafe_disable();
copter.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 HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -173,31 +170,33 @@ bool Copter::init_arm_motors(const AP_Arming::Method method, const bool do_armin
// Remember Orientation
// --------------------
init_simple_bearing();
copter.init_simple_bearing();
initial_armed_bearing = ahrs.yaw_sensor;
AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
copter.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);
AP::logger().Write_Event(DATA_EKF_ALT_RESET);
// we have reset height, so arming height is zero
arming_altitude_m = 0;
copter.arming_altitude_m = 0;
} else if (!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 (!copter.set_home_to_current_location(false)) {
// ignore failure
}
// remember the height when we armed
arming_altitude_m = inertial_nav.get_altitude() * 0.01;
copter.arming_altitude_m = copter.inertial_nav.get_altitude() * 0.01;
}
update_super_simple_bearing(false);
copter.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());
copter.g2.smart_rtl.set_home(copter.position_ok());
#endif
// enable gps velocity based centrefugal force compensation
@ -206,55 +205,62 @@ bool Copter::init_arm_motors(const AP_Arming::Method method, const bool do_armin
#if SPRAYER_ENABLED == ENABLED
// turn off sprayer's test if on
sprayer.test_pump(false);
copter.sprayer.test_pump(false);
#endif
// enable output to motors
enable_motor_output();
copter.enable_motor_output();
// finally actually arm the motors
motors->armed(true);
copter.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(copter.control_mode, copter.control_mode_reason);
// re-enable failsafe
failsafe_enable();
copter.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;
// Log time stamp of arming event
arm_time_ms = millis();
copter.arm_time_ms = millis();
// Start the arming delay
ap.in_arming_delay = true;
copter.ap.in_arming_delay = true;
// assumed armed without a arming, switch. Overridden in switches.cpp
ap.armed_with_switch = false;
copter.ap.armed_with_switch = false;
// return success
return true;
}
// init_disarm_motors - disarm motors
void Copter::init_disarm_motors()
// arming.disarm - disarm motors
bool AP_Arming_Copter::disarm()
{
// return immediately if we are already disarmed
if (!motors->armed()) {
return;
if (!copter.motors->armed()) {
return true;
}
if (!AP_Arming::disarm()) {
return false;
}
#if HIL_MODE != HIL_MODE_DISABLED || 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
Compass &compass = AP::compass();
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
Vector3f magOffsets;
@ -266,25 +272,25 @@ void Copter::init_disarm_motors()
#if AUTOTUNE_ENABLED == ENABLED
// save auto tuned parameters
if (control_mode == AUTOTUNE) {;
mode_autotune.save_tuning_gains();
if (copter.flightmode == &copter.mode_autotune) {
copter.mode_autotune.save_tuning_gains();
} else {
mode_autotune.reset();
copter.mode_autotune.reset();
}
#endif
// we are not in the air
set_land_complete(true);
set_land_complete_maybe(true);
copter.set_land_complete(true);
copter.set_land_complete_maybe(true);
Log_Write_Event(DATA_DISARMED);
AP::logger().Write_Event(DATA_DISARMED);
// send disarm command to motors
motors->armed(false);
copter.motors->armed(false);
#if MODE_AUTO_ENABLED == ENABLED
// reset the mission
mode_auto.mission.reset();
copter.mode_auto.mission.reset();
#endif
AP::logger().set_vehicle_armed(false);
@ -293,7 +299,9 @@ void Copter::init_disarm_motors()
ahrs.set_correct_centrifugal(false);
hal.util->set_soft_armed(false);
ap.in_arming_delay = false;
copter.ap.in_arming_delay = false;
return true;
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos

10
ArduCopter/toy_mode.cpp
View File

@ -424,7 +424,7 @@ void ToyMode::update()
if (throttle_high_counter >= TOY_LAND_ARM_COUNT) {
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: throttle arm");
arm_check_compass();
if (!copter.init_arm_motors(AP_Arming::Method::MAVLINK) && (flags & FLAG_UPGRADE_LOITER) && copter.control_mode == LOITER) {
if (!copter.arming.arm(AP_Arming::Method::MAVLINK) && (flags & FLAG_UPGRADE_LOITER) && copter.control_mode == LOITER) {
/*
support auto-switching to ALT_HOLD, then upgrade to LOITER once GPS available
*/
@ -433,7 +433,7 @@ void ToyMode::update()
#if AC_FENCE == ENABLED
copter.fence.enable(false);
#endif
if (!copter.init_arm_motors(AP_Arming::Method::MAVLINK)) {
if (!copter.arming.arm(AP_Arming::Method::MAVLINK)) {
// go back to LOITER
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: ALT_HOLD arm failed");
set_and_remember_mode(LOITER, MODE_REASON_TMODE);
@ -625,7 +625,7 @@ void ToyMode::update()
#if AC_FENCE == ENABLED
copter.fence.enable(false);
#endif
if (copter.init_arm_motors(AP_Arming::Method::MAVLINK)) {
if (copter.arming.arm(AP_Arming::Method::MAVLINK)) {
load_test.running = true;
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: load_test on");
} else {
@ -803,7 +803,7 @@ void ToyMode::action_arm(void)
// we want GPS and checks are passing, arm and enable fence
copter.fence.enable(true);
#endif
copter.init_arm_motors(AP_Arming::Method::RUDDER);
copter.arming.arm(AP_Arming::Method::RUDDER);
if (!copter.motors->armed()) {
AP_Notify::events.arming_failed = true;
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: GPS arming failed");
@ -819,7 +819,7 @@ void ToyMode::action_arm(void)
// non-GPS mode
copter.fence.enable(false);
#endif
copter.init_arm_motors(AP_Arming::Method::RUDDER);
copter.arming.arm(AP_Arming::Method::RUDDER);
if (!copter.motors->armed()) {
AP_Notify::events.arming_failed = true;
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: non-GPS arming failed");