mirror of https://github.com/ArduPilot/ardupilot
Copter: use AP_Arming methods to arm and disarm vehicle
Really just changing the namespace of init_arm_motors
This commit is contained in:
parent
61a2be1470
commit
6dce39cbe1
|
@ -12,20 +12,19 @@ void AP_Arming_Copter::update(void)
|
||||||
pre_arm_display_counter = 0;
|
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::pre_arm_checks(bool display_failure)
|
||||||
bool AP_Arming_Copter::all_checks_passing(AP_Arming::Method method)
|
|
||||||
{
|
{
|
||||||
set_pre_arm_check(pre_arm_checks(true));
|
const bool passed = run_pre_arm_checks(display_failure);
|
||||||
|
set_pre_arm_check(passed);
|
||||||
return copter.ap.pre_arm_check && arm_checks(method);
|
return passed;
|
||||||
}
|
}
|
||||||
|
|
||||||
// perform pre-arm checks
|
// perform pre-arm checks
|
||||||
// return true if the checks pass successfully
|
// 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
|
// exit immediately if already armed
|
||||||
if (copter.motors->armed()) {
|
if (copter.motors->armed()) {
|
||||||
|
|
|
@ -20,10 +20,12 @@ public:
|
||||||
AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete;
|
AP_Arming_Copter &operator=(const AP_Arming_Copter&) = delete;
|
||||||
|
|
||||||
void update(void);
|
void update(void);
|
||||||
bool all_checks_passing(AP_Arming::Method method);
|
|
||||||
|
|
||||||
bool rc_calibration_checks(bool display_failure) override;
|
bool rc_calibration_checks(bool display_failure) override;
|
||||||
|
|
||||||
|
bool disarm() override;
|
||||||
|
bool arm(AP_Arming::Method method, bool do_arming_checks=true) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
||||||
bool pre_arm_checks(bool display_failure) override;
|
bool pre_arm_checks(bool display_failure) override;
|
||||||
|
@ -48,4 +50,8 @@ protected:
|
||||||
|
|
||||||
private:
|
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);
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -752,8 +752,6 @@ private:
|
||||||
// motors.cpp
|
// motors.cpp
|
||||||
void arm_motors_check();
|
void arm_motors_check();
|
||||||
void auto_disarm_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 motors_output();
|
||||||
void lost_vehicle_check();
|
void lost_vehicle_check();
|
||||||
|
|
||||||
|
|
|
@ -606,6 +606,11 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_mount(const mavlink_command_long_t
|
||||||
return GCS_MAVLINK::handle_command_mount(packet);
|
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)
|
MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_long_t &packet)
|
||||||
{
|
{
|
||||||
switch(packet.command) {
|
switch(packet.command) {
|
||||||
|
@ -699,26 +704,6 @@ MAV_RESULT GCS_MAVLINK_Copter::handle_command_long_packet(const mavlink_command_
|
||||||
return MAV_RESULT_FAILED;
|
return MAV_RESULT_FAILED;
|
||||||
#endif
|
#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
|
#if PARACHUTE == ENABLED
|
||||||
case MAV_CMD_DO_PARACHUTE:
|
case MAV_CMD_DO_PARACHUTE:
|
||||||
// configure or release 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 (!copter.motors->armed()) {
|
||||||
// if disarmed, arm motors
|
// 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) {
|
} else if (copter.ap.land_complete) {
|
||||||
// if armed and landed, takeoff
|
// if armed and landed, takeoff
|
||||||
if (copter.set_mode(LOITER, MODE_REASON_GCS_COMMAND)) {
|
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.motors->armed()) {
|
||||||
if (copter.ap.land_complete) {
|
if (copter.ap.land_complete) {
|
||||||
// if landed, disarm motors
|
// if landed, disarm motors
|
||||||
copter.init_disarm_motors();
|
copter.arming.disarm();
|
||||||
} else {
|
} else {
|
||||||
// assume that shots modes are all done in guided.
|
// assume that shots modes are all done in guided.
|
||||||
// NOTE: this may need to change if we add a non-guided shot mode
|
// 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) {
|
if (GCS_MAVLINK::handle_flight_termination(packet) != MAV_RESULT_ACCEPTED) {
|
||||||
#endif
|
#endif
|
||||||
if (packet.param1 > 0.5f) {
|
if (packet.param1 > 0.5f) {
|
||||||
copter.init_disarm_motors();
|
copter.arming.disarm();
|
||||||
result = MAV_RESULT_ACCEPTED;
|
result = MAV_RESULT_ACCEPTED;
|
||||||
}
|
}
|
||||||
#if ADVANCED_FAILSAFE == ENABLED
|
#if ADVANCED_FAILSAFE == ENABLED
|
||||||
|
|
|
@ -43,6 +43,8 @@ protected:
|
||||||
virtual MAV_VTOL_STATE vtol_state() const override { return MAV_VTOL_STATE_MC; };
|
virtual MAV_VTOL_STATE vtol_state() const override { return MAV_VTOL_STATE_MC; };
|
||||||
virtual MAV_LANDED_STATE landed_state() const override;
|
virtual MAV_LANDED_STATE landed_state() const override;
|
||||||
|
|
||||||
|
bool allow_disarm() const override;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
void handleMessage(mavlink_message_t * msg) override;
|
void handleMessage(mavlink_message_t * msg) override;
|
||||||
|
|
|
@ -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
|
// arm or disarm the vehicle
|
||||||
switch (ch_flag) {
|
switch (ch_flag) {
|
||||||
case HIGH:
|
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
|
// remember that we are using an arming switch, for use by set_throttle_zero_flag
|
||||||
copter.ap.armed_with_switch = true;
|
copter.ap.armed_with_switch = true;
|
||||||
break;
|
break;
|
||||||
|
@ -428,7 +428,7 @@ void RC_Channel_Copter::do_aux_function(const aux_func_t ch_option, const aux_sw
|
||||||
// nothing
|
// nothing
|
||||||
break;
|
break;
|
||||||
case LOW:
|
case LOW:
|
||||||
copter.init_disarm_motors();
|
copter.arming.disarm();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
|
@ -25,7 +25,7 @@ void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
|
||||||
copter.motors->output();
|
copter.motors->output();
|
||||||
|
|
||||||
// disarm as well
|
// disarm as well
|
||||||
copter.init_disarm_motors();
|
copter.arming.disarm();
|
||||||
|
|
||||||
// and set all aux channels
|
// and set all aux channels
|
||||||
SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
SRV_Channels::set_output_limit(SRV_Channel::k_heli_rsc, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM);
|
||||||
|
|
|
@ -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 landed and we will take some kind of action, just disarm
|
||||||
if ((actual_action > MAV_COLLISION_ACTION_REPORT) && copter.should_disarm_on_failsafe()) {
|
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;
|
actual_action = MAV_COLLISION_ACTION_NONE;
|
||||||
} else {
|
} else {
|
||||||
|
|
||||||
|
|
|
@ -53,7 +53,7 @@ void Copter::crash_check()
|
||||||
// send message to gcs
|
// send message to gcs
|
||||||
gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
|
gcs().send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
|
||||||
// disarm motors
|
// disarm motors
|
||||||
init_disarm_motors();
|
copter.arming.disarm();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -210,7 +210,7 @@ void Copter::parachute_check()
|
||||||
void Copter::parachute_release()
|
void Copter::parachute_release()
|
||||||
{
|
{
|
||||||
// disarm motors
|
// disarm motors
|
||||||
init_disarm_motors();
|
arming.disarm();
|
||||||
|
|
||||||
// release parachute
|
// release parachute
|
||||||
parachute.release();
|
parachute.release();
|
||||||
|
|
|
@ -12,7 +12,7 @@ void Copter::failsafe_radio_on_event()
|
||||||
}
|
}
|
||||||
|
|
||||||
if (should_disarm_on_failsafe()) {
|
if (should_disarm_on_failsafe()) {
|
||||||
init_disarm_motors();
|
arming.disarm();
|
||||||
} else {
|
} else {
|
||||||
if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
|
if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
|
||||||
// continue mission
|
// continue mission
|
||||||
|
@ -55,7 +55,7 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
|
||||||
|
|
||||||
// failsafe check
|
// failsafe check
|
||||||
if (should_disarm_on_failsafe()) {
|
if (should_disarm_on_failsafe()) {
|
||||||
init_disarm_motors();
|
arming.disarm();
|
||||||
} else {
|
} else {
|
||||||
switch ((Failsafe_Action)action) {
|
switch ((Failsafe_Action)action) {
|
||||||
case Failsafe_Action_None:
|
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);
|
snprintf(battery_type_str, 17, "%s battery", type_str);
|
||||||
g2.afs.gcs_terminate(true, battery_type_str);
|
g2.afs.gcs_terminate(true, battery_type_str);
|
||||||
#else
|
#else
|
||||||
init_disarm_motors();
|
arming.disarm();
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -122,7 +122,7 @@ void Copter::failsafe_gcs_check()
|
||||||
RC_Channels::clear_overrides();
|
RC_Channels::clear_overrides();
|
||||||
|
|
||||||
if (should_disarm_on_failsafe()) {
|
if (should_disarm_on_failsafe()) {
|
||||||
init_disarm_motors();
|
arming.disarm();
|
||||||
} else {
|
} else {
|
||||||
if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
|
if (control_mode == AUTO && g.failsafe_gcs == FS_GCS_ENABLED_CONTINUE_MISSION) {
|
||||||
// continue mission
|
// continue mission
|
||||||
|
@ -190,7 +190,7 @@ void Copter::failsafe_terrain_on_event()
|
||||||
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
|
AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_TERRAIN, LogErrorCode::FAILSAFE_OCCURRED);
|
||||||
|
|
||||||
if (should_disarm_on_failsafe()) {
|
if (should_disarm_on_failsafe()) {
|
||||||
init_disarm_motors();
|
arming.disarm();
|
||||||
#if MODE_RTL_ENABLED == ENABLED
|
#if MODE_RTL_ENABLED == ENABLED
|
||||||
} else if (control_mode == RTL) {
|
} else if (control_mode == RTL) {
|
||||||
mode_rtl.restart_without_terrain();
|
mode_rtl.restart_without_terrain();
|
||||||
|
|
|
@ -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
|
// 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
|
// 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))){
|
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 {
|
} else {
|
||||||
|
|
||||||
|
|
|
@ -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();
|
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) {
|
if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
|
||||||
init_disarm_motors();
|
arming.disarm();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -537,7 +537,7 @@ void Copter::ModeAuto::exit_mission()
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
// if we've landed it's safe to disarm
|
// if we've landed it's safe to disarm
|
||||||
copter.init_disarm_motors();
|
copter.arming.disarm();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -55,7 +55,7 @@ void Copter::ModeLand::gps_run()
|
||||||
{
|
{
|
||||||
// disarm when the landing detector says we've landed
|
// disarm when the landing detector says we've landed
|
||||||
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
|
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
|
||||||
copter.init_disarm_motors();
|
copter.arming.disarm();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Land State Machine Determination
|
// Land State Machine Determination
|
||||||
|
@ -109,7 +109,7 @@ void Copter::ModeLand::nogps_run()
|
||||||
|
|
||||||
// disarm when the landing detector says we've landed
|
// disarm when the landing detector says we've landed
|
||||||
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
|
if (ap.land_complete && motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE) {
|
||||||
copter.init_disarm_motors();
|
copter.arming.disarm();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Land State Machine Determination
|
// Land State Machine Determination
|
||||||
|
|
|
@ -378,7 +378,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
|
||||||
|
|
||||||
// disarm when the landing detector says we've landed
|
// 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) {
|
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
|
// if not armed set throttle to zero and exit immediately
|
||||||
|
|
|
@ -45,7 +45,7 @@ void Copter::arm_motors_check()
|
||||||
// arm the motors and configure for flight
|
// arm the motors and configure for flight
|
||||||
if (arming_counter == ARM_DELAY && !motors->armed()) {
|
if (arming_counter == ARM_DELAY && !motors->armed()) {
|
||||||
// reset arming counter if arming fail
|
// reset arming counter if arming fail
|
||||||
if (!init_arm_motors(AP_Arming::Method::RUDDER)) {
|
if (!arming.arm(AP_Arming::Method::RUDDER)) {
|
||||||
arming_counter = 0;
|
arming_counter = 0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -71,7 +71,7 @@ void Copter::arm_motors_check()
|
||||||
|
|
||||||
// disarm the motors
|
// disarm the motors
|
||||||
if (arming_counter == DISARM_DELAY && motors->armed()) {
|
if (arming_counter == DISARM_DELAY && motors->armed()) {
|
||||||
init_disarm_motors();
|
arming.disarm();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Yaw is centered so reset arming counter
|
// Yaw is centered so reset arming counter
|
||||||
|
@ -124,14 +124,12 @@ void Copter::auto_disarm_check()
|
||||||
|
|
||||||
// disarm once timer expires
|
// disarm once timer expires
|
||||||
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
|
if ((tnow_ms-auto_disarm_begin) >= disarm_delay_ms) {
|
||||||
init_disarm_motors();
|
arming.disarm();
|
||||||
auto_disarm_begin = tnow_ms;
|
auto_disarm_begin = tnow_ms;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// init_arm_motors - performs arming process including initialisation of barometer and gyros
|
bool AP_Arming_Copter::arm(const AP_Arming::Method method, const bool do_arming_checks)
|
||||||
// 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)
|
|
||||||
{
|
{
|
||||||
static bool in_arm_motors = false;
|
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;
|
in_arm_motors = true;
|
||||||
|
|
||||||
// return true if already armed
|
// return true if already armed
|
||||||
if (motors->armed()) {
|
if (copter.motors->armed()) {
|
||||||
in_arm_motors = false;
|
in_arm_motors = false;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// run pre-arm-checks and display failures
|
if (!AP_Arming::arm(method, do_arming_checks)) {
|
||||||
if (do_arming_checks && !arming.all_checks_passing(method)) {
|
|
||||||
AP_Notify::events.arming_failed = true;
|
AP_Notify::events.arming_failed = true;
|
||||||
in_arm_motors = false;
|
in_arm_motors = false;
|
||||||
return 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);
|
AP::logger().set_vehicle_armed(true);
|
||||||
|
|
||||||
// disable cpu failsafe because initialising everything takes a while
|
// 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)
|
// notify that arming will occur (we do this early to give plenty of warning)
|
||||||
AP_Notify::flags.armed = true;
|
AP_Notify::flags.armed = true;
|
||||||
// call notify update a few times to ensure the message gets out
|
// call notify update a few times to ensure the message gets out
|
||||||
for (uint8_t i=0; i<=10; i++) {
|
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
|
#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
|
// 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()) {
|
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)
|
// Reset EKF altitude if home hasn't been set yet (we use EKF altitude as substitute for alt above home)
|
||||||
ahrs.resetHeightDatum();
|
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
|
// we have reset height, so arming height is zero
|
||||||
arming_altitude_m = 0;
|
copter.arming_altitude_m = 0;
|
||||||
} else if (!ahrs.home_is_locked()) {
|
} else if (!ahrs.home_is_locked()) {
|
||||||
// Reset home position if it has already been set before (but not 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
|
// ignore failure
|
||||||
}
|
}
|
||||||
|
|
||||||
// remember the height when we armed
|
// 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
|
// Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point
|
||||||
#if MODE_SMARTRTL_ENABLED == ENABLED
|
#if MODE_SMARTRTL_ENABLED == ENABLED
|
||||||
g2.smart_rtl.set_home(position_ok());
|
copter.g2.smart_rtl.set_home(copter.position_ok());
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// enable gps velocity based centrefugal force compensation
|
// 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
|
#if SPRAYER_ENABLED == ENABLED
|
||||||
// turn off sprayer's test if on
|
// turn off sprayer's test if on
|
||||||
sprayer.test_pump(false);
|
copter.sprayer.test_pump(false);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// enable output to motors
|
// enable output to motors
|
||||||
enable_motor_output();
|
copter.enable_motor_output();
|
||||||
|
|
||||||
// finally actually arm the motors
|
// 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
|
// 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
|
// re-enable failsafe
|
||||||
failsafe_enable();
|
copter.failsafe_enable();
|
||||||
|
|
||||||
// perf monitor ignores delay due to arming
|
// perf monitor ignores delay due to arming
|
||||||
scheduler.perf_info.ignore_this_loop();
|
AP::scheduler().perf_info.ignore_this_loop();
|
||||||
|
|
||||||
// flag exiting this function
|
// flag exiting this function
|
||||||
in_arm_motors = false;
|
in_arm_motors = false;
|
||||||
|
|
||||||
// Log time stamp of arming event
|
// Log time stamp of arming event
|
||||||
arm_time_ms = millis();
|
copter.arm_time_ms = millis();
|
||||||
|
|
||||||
// Start the arming delay
|
// 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
|
// assumed armed without a arming, switch. Overridden in switches.cpp
|
||||||
ap.armed_with_switch = false;
|
copter.ap.armed_with_switch = false;
|
||||||
|
|
||||||
// return success
|
// return success
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
// init_disarm_motors - disarm motors
|
// arming.disarm - disarm motors
|
||||||
void Copter::init_disarm_motors()
|
bool AP_Arming_Copter::disarm()
|
||||||
{
|
{
|
||||||
// return immediately if we are already disarmed
|
// return immediately if we are already disarmed
|
||||||
if (!motors->armed()) {
|
if (!copter.motors->armed()) {
|
||||||
return;
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!AP_Arming::disarm()) {
|
||||||
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors");
|
gcs().send_text(MAV_SEVERITY_INFO, "Disarming motors");
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
|
||||||
|
|
||||||
// save compass offsets learned by the EKF if enabled
|
// save compass offsets learned by the EKF if enabled
|
||||||
|
Compass &compass = AP::compass();
|
||||||
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
|
if (ahrs.use_compass() && compass.get_learn_type() == Compass::LEARN_EKF) {
|
||||||
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
for(uint8_t i=0; i<COMPASS_MAX_INSTANCES; i++) {
|
||||||
Vector3f magOffsets;
|
Vector3f magOffsets;
|
||||||
|
@ -266,25 +272,25 @@ void Copter::init_disarm_motors()
|
||||||
|
|
||||||
#if AUTOTUNE_ENABLED == ENABLED
|
#if AUTOTUNE_ENABLED == ENABLED
|
||||||
// save auto tuned parameters
|
// save auto tuned parameters
|
||||||
if (control_mode == AUTOTUNE) {;
|
if (copter.flightmode == &copter.mode_autotune) {
|
||||||
mode_autotune.save_tuning_gains();
|
copter.mode_autotune.save_tuning_gains();
|
||||||
} else {
|
} else {
|
||||||
mode_autotune.reset();
|
copter.mode_autotune.reset();
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// we are not in the air
|
// we are not in the air
|
||||||
set_land_complete(true);
|
copter.set_land_complete(true);
|
||||||
set_land_complete_maybe(true);
|
copter.set_land_complete_maybe(true);
|
||||||
|
|
||||||
Log_Write_Event(DATA_DISARMED);
|
AP::logger().Write_Event(DATA_DISARMED);
|
||||||
|
|
||||||
// send disarm command to motors
|
// send disarm command to motors
|
||||||
motors->armed(false);
|
copter.motors->armed(false);
|
||||||
|
|
||||||
#if MODE_AUTO_ENABLED == ENABLED
|
#if MODE_AUTO_ENABLED == ENABLED
|
||||||
// reset the mission
|
// reset the mission
|
||||||
mode_auto.mission.reset();
|
copter.mode_auto.mission.reset();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
AP::logger().set_vehicle_armed(false);
|
AP::logger().set_vehicle_armed(false);
|
||||||
|
@ -293,7 +299,9 @@ void Copter::init_disarm_motors()
|
||||||
ahrs.set_correct_centrifugal(false);
|
ahrs.set_correct_centrifugal(false);
|
||||||
hal.util->set_soft_armed(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
|
// motors_output - send output to motors library which will adjust and send to ESCs and servos
|
||||||
|
|
|
@ -424,7 +424,7 @@ void ToyMode::update()
|
||||||
if (throttle_high_counter >= TOY_LAND_ARM_COUNT) {
|
if (throttle_high_counter >= TOY_LAND_ARM_COUNT) {
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: throttle arm");
|
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: throttle arm");
|
||||||
arm_check_compass();
|
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
|
support auto-switching to ALT_HOLD, then upgrade to LOITER once GPS available
|
||||||
*/
|
*/
|
||||||
|
@ -433,7 +433,7 @@ void ToyMode::update()
|
||||||
#if AC_FENCE == ENABLED
|
#if AC_FENCE == ENABLED
|
||||||
copter.fence.enable(false);
|
copter.fence.enable(false);
|
||||||
#endif
|
#endif
|
||||||
if (!copter.init_arm_motors(AP_Arming::Method::MAVLINK)) {
|
if (!copter.arming.arm(AP_Arming::Method::MAVLINK)) {
|
||||||
// go back to LOITER
|
// go back to LOITER
|
||||||
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: ALT_HOLD arm failed");
|
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: ALT_HOLD arm failed");
|
||||||
set_and_remember_mode(LOITER, MODE_REASON_TMODE);
|
set_and_remember_mode(LOITER, MODE_REASON_TMODE);
|
||||||
|
@ -625,7 +625,7 @@ void ToyMode::update()
|
||||||
#if AC_FENCE == ENABLED
|
#if AC_FENCE == ENABLED
|
||||||
copter.fence.enable(false);
|
copter.fence.enable(false);
|
||||||
#endif
|
#endif
|
||||||
if (copter.init_arm_motors(AP_Arming::Method::MAVLINK)) {
|
if (copter.arming.arm(AP_Arming::Method::MAVLINK)) {
|
||||||
load_test.running = true;
|
load_test.running = true;
|
||||||
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: load_test on");
|
gcs().send_text(MAV_SEVERITY_INFO, "Tmode: load_test on");
|
||||||
} else {
|
} else {
|
||||||
|
@ -803,7 +803,7 @@ void ToyMode::action_arm(void)
|
||||||
// we want GPS and checks are passing, arm and enable fence
|
// we want GPS and checks are passing, arm and enable fence
|
||||||
copter.fence.enable(true);
|
copter.fence.enable(true);
|
||||||
#endif
|
#endif
|
||||||
copter.init_arm_motors(AP_Arming::Method::RUDDER);
|
copter.arming.arm(AP_Arming::Method::RUDDER);
|
||||||
if (!copter.motors->armed()) {
|
if (!copter.motors->armed()) {
|
||||||
AP_Notify::events.arming_failed = true;
|
AP_Notify::events.arming_failed = true;
|
||||||
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: GPS arming failed");
|
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: GPS arming failed");
|
||||||
|
@ -819,7 +819,7 @@ void ToyMode::action_arm(void)
|
||||||
// non-GPS mode
|
// non-GPS mode
|
||||||
copter.fence.enable(false);
|
copter.fence.enable(false);
|
||||||
#endif
|
#endif
|
||||||
copter.init_arm_motors(AP_Arming::Method::RUDDER);
|
copter.arming.arm(AP_Arming::Method::RUDDER);
|
||||||
if (!copter.motors->armed()) {
|
if (!copter.motors->armed()) {
|
||||||
AP_Notify::events.arming_failed = true;
|
AP_Notify::events.arming_failed = true;
|
||||||
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: non-GPS arming failed");
|
gcs().send_text(MAV_SEVERITY_ERROR, "Tmode: non-GPS arming failed");
|
||||||
|
|
Loading…
Reference in New Issue