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

Copter: add configurable arming delay

This commit is contained in:
Jonathan Challinger 2016-01-21 18:59:47 -08:00 committed by Randy Mackay
parent 4a6cc75ebb
commit 80f3541933
8 changed files with 43 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduCopter/Copter.h
View File

@ -248,6 +248,8 @@ private:
uint8_t using_interlock : 1; // 20 // aux switch motor interlock function is in use
uint8_t motor_emergency_stop : 1; // 21 // motor estop switch, shuts off motors when enabled
uint8_t land_repo_active : 1; // 22 // true if the pilot is overriding the landing position
uint8_t motor_interlock_switch : 1; // 23 // true if pilot is requesting motor interlock enable
uint8_t in_arming_delay : 1; // 24 // true while we are armed but waiting to spin motors
};
uint32_t value;
} ap;
@ -489,6 +491,8 @@ private:
uint16_t mainLoop_count;
// Loiter timer - Records how long we have been in loiter
uint32_t rtl_loiter_start_time;
// arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed.
uint32_t arm_time_ms;
// Used to exit the roll and pitch auto trim function
uint8_t auto_trim_counter;

5
ArduCopter/arming_checks.cpp
View File

@ -24,6 +24,8 @@ bool Copter::all_arming_checks_passing(bool arming_from_gcs)
{
if (pre_arm_checks(true)) {
set_pre_arm_check(true);
} else {
return false;
}
return ap.pre_arm_check && arm_checks(true, arming_from_gcs);
@ -51,8 +53,7 @@ bool Copter::pre_arm_checks(bool display_failure)
// if it is, switch needs to be in disabled position to arm
// otherwise exit immediately. This check to be repeated,
// as state can change at any time.
// skip check in Throw mode which takes control of the motor interlock
if (ap.using_interlock && motors.get_interlock()) {
if (ap.using_interlock && ap.motor_interlock_switch) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled");
}

4
ArduCopter/config.h
View File

@ -54,6 +54,10 @@
# define MAIN_LOOP_SECONDS 0.0025f
# define MAIN_LOOP_MICROS 2500
#ifndef ARMING_DELAY_SEC
# define ARMING_DELAY_SEC 2.0f
#endif
//////////////////////////////////////////////////////////////////////////////
// FRAME_CONFIG
//

2
ArduCopter/control_throw.cpp
View File

@ -11,7 +11,7 @@ bool Copter::throw_init(bool ignore_checks)
return false;
#endif
// do not enter the mode when already armed
// do not enter the mode when already armed or when flying
if (motors.armed()) {
return false;
}

4
ArduCopter/events.cpp
View File

@ -201,6 +201,10 @@ void Copter::set_mode_RTL_or_land_with_pause(mode_reason_t reason)
}
bool Copter::should_disarm_on_failsafe() {
if (ap.in_arming_delay) {
return true;
}
switch(control_mode) {
case STABILIZE:
case ACRO:

8
ArduCopter/heli.cpp
View File

@ -147,10 +147,10 @@ void Copter::heli_update_rotor_speed_targets()
case ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH:
// pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom
if (rsc_control_deglitched > 0.01f) {
motors.set_interlock(true);
ap.motor_interlock_switch = true;
motors.set_desired_rotor_speed(rsc_control_deglitched);
} else {
motors.set_interlock(false);
ap.motor_interlock_switch = false;
motors.set_desired_rotor_speed(0.0f);
}
break;
@ -160,10 +160,10 @@ void Copter::heli_update_rotor_speed_targets()
// pass setpoint through as desired rotor speed, this is almost pointless as the Setpoint serves no function in this mode
// other than being used to create a crude estimate of rotor speed
if (rsc_control_deglitched > 0.0f) {
motors.set_interlock(true);
ap.motor_interlock_switch = true;
motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
}else{
motors.set_interlock(false);
ap.motor_interlock_switch = false;
motors.set_desired_rotor_speed(0.0f);
}
break;

28
ArduCopter/motors.cpp
View File

@ -202,6 +202,12 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
// 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;
}
@ -254,22 +260,32 @@ void Copter::init_disarm_motors()
// disable gps velocity based centrefugal force compensation
ahrs.set_correct_centrifugal(false);
hal.util->set_soft_armed(false);
ap.in_arming_delay = false;
}
// motors_output - send output to motors library which will adjust and send to ESCs and servos
void Copter::motors_output()
{
// Update arming delay state
if (ap.in_arming_delay && (!motors.armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
ap.in_arming_delay = false;
}
// check if we are performing the motor test
if (ap.motor_test) {
motor_test_output();
} else {
if (!ap.using_interlock){
// if not using interlock switch, set according to Emergency Stop status
// where Emergency Stop is forced false during arming if Emergency Stop switch
// is not used. Interlock enabled means motors run, so we must
// invert motor_emergency_stop status for motors to run.
motors.set_interlock(!ap.motor_emergency_stop);
bool interlock = motors.armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop;
if (!motors.get_interlock() && interlock) {
motors.set_interlock(true);
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
} else if (motors.get_interlock() && !interlock) {
motors.set_interlock(false);
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
}
// send output signals to motors
motors.output();
}
}

9
ArduCopter/switches.cpp
View File

@ -565,14 +565,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
case AUXSW_MOTOR_INTERLOCK:
// Turn on when above LOW, because channel will also be used for speed
// control signal in tradheli
motors.set_interlock(ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
// Log new status
if (motors.get_interlock()){
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
} else {
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
}
ap.motor_interlock_switch = (ch_flag == AUX_SWITCH_HIGH || ch_flag == AUX_SWITCH_MIDDLE);
break;
case AUXSW_BRAKE: