mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 23:48:31 -04:00
71cbcacf50
Also fixed bug in which vehicle could appear to be armed (according to LEDs) but is not. This could occur if pilot had raised the throttle very high during the calibration that happens just before arming.
579 lines
18 KiB
Plaintext
579 lines
18 KiB
Plaintext
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
#define ARM_DELAY 20 // called at 10hz so 2 seconds
|
|
#define DISARM_DELAY 20 // called at 10hz so 2 seconds
|
|
#define AUTO_TRIM_DELAY 100 // called at 10hz so 10 seconds
|
|
#define AUTO_DISARMING_DELAY 15 // called at 1hz so 15 seconds
|
|
|
|
// arm_motors_check - checks for pilot input to arm or disarm the copter
|
|
// called at 10hz
|
|
static void arm_motors_check()
|
|
{
|
|
static int16_t arming_counter;
|
|
bool allow_arming = false;
|
|
|
|
// ensure throttle is down
|
|
if (g.rc_3.control_in > 0) {
|
|
arming_counter = 0;
|
|
return;
|
|
}
|
|
|
|
// allow arming/disarming in fully manual flight modes ACRO, STABILIZE, SPORT and DRIFT
|
|
if (manual_flight_mode(control_mode)) {
|
|
allow_arming = true;
|
|
}
|
|
|
|
// allow arming/disarming in Loiter and AltHold if landed
|
|
if (ap.land_complete && (control_mode == LOITER || control_mode == ALT_HOLD || control_mode == POSHOLD || control_mode == AUTOTUNE)) {
|
|
allow_arming = true;
|
|
}
|
|
|
|
// kick out other flight modes
|
|
if (!allow_arming) {
|
|
arming_counter = 0;
|
|
return;
|
|
}
|
|
|
|
#if FRAME_CONFIG == HELI_FRAME
|
|
// heli specific arming check
|
|
if (!motors.allow_arming()){
|
|
arming_counter = 0;
|
|
return;
|
|
}
|
|
#endif // HELI_FRAME
|
|
|
|
int16_t tmp = g.rc_4.control_in;
|
|
|
|
// full right
|
|
if (tmp > 4000) {
|
|
|
|
// increase the arming counter to a maximum of 1 beyond the auto trim counter
|
|
if( arming_counter <= AUTO_TRIM_DELAY ) {
|
|
arming_counter++;
|
|
}
|
|
|
|
// arm the motors and configure for flight
|
|
if (arming_counter == ARM_DELAY && !motors.armed()) {
|
|
// run pre-arm-checks and display failures
|
|
pre_arm_checks(true);
|
|
if(ap.pre_arm_check && arm_checks(true)) {
|
|
init_arm_motors();
|
|
}else{
|
|
// reset arming counter if pre-arm checks fail
|
|
arming_counter = 0;
|
|
AP_Notify::flags.arming_failed = true;
|
|
}
|
|
}
|
|
|
|
// arm the motors and configure for flight
|
|
if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) {
|
|
auto_trim_counter = 250;
|
|
}
|
|
|
|
// full left
|
|
}else if (tmp < -4000) {
|
|
|
|
// increase the counter to a maximum of 1 beyond the disarm delay
|
|
if( arming_counter <= DISARM_DELAY ) {
|
|
arming_counter++;
|
|
}
|
|
|
|
// disarm the motors
|
|
if (arming_counter == DISARM_DELAY && motors.armed()) {
|
|
init_disarm_motors();
|
|
}
|
|
|
|
// Yaw is centered so reset arming counter
|
|
}else{
|
|
AP_Notify::flags.arming_failed = false;
|
|
arming_counter = 0;
|
|
}
|
|
}
|
|
|
|
// auto_disarm_check - disarms the copter if it has been sitting on the ground in manual mode with throttle low for at least 15 seconds
|
|
// called at 1hz
|
|
static void auto_disarm_check()
|
|
{
|
|
static uint8_t auto_disarming_counter;
|
|
|
|
// exit immediately if we are already disarmed or throttle is not zero
|
|
if (!motors.armed() || g.rc_3.control_in > 0) {
|
|
auto_disarming_counter = 0;
|
|
return;
|
|
}
|
|
|
|
// allow auto disarm in manual flight modes or Loiter/AltHold if we're landed
|
|
if(manual_flight_mode(control_mode) || (ap.land_complete && (control_mode == LOITER || control_mode == ALT_HOLD || control_mode == POSHOLD || control_mode == AUTOTUNE))) {
|
|
auto_disarming_counter++;
|
|
|
|
if(auto_disarming_counter >= AUTO_DISARMING_DELAY) {
|
|
init_disarm_motors();
|
|
auto_disarming_counter = 0;
|
|
}
|
|
}else{
|
|
auto_disarming_counter = 0;
|
|
}
|
|
}
|
|
|
|
// init_arm_motors - performs arming process including initialisation of barometer and gyros
|
|
static void init_arm_motors()
|
|
{
|
|
// arming marker
|
|
// Flag used to track if we have armed the motors the first time.
|
|
// This is used to decide if we should run the ground_start routine
|
|
// which calibrates the IMU
|
|
static bool did_ground_start = false;
|
|
|
|
// disable cpu failsafe because initialising everything takes a while
|
|
failsafe_disable();
|
|
|
|
// disable inertial nav errors temporarily
|
|
inertial_nav.ignore_next_error();
|
|
|
|
// notify that arming will occur (we do this early to give plenty of warning)
|
|
AP_Notify::flags.armed = true;
|
|
// call update_notify a few times to ensure the message gets out
|
|
for (uint8_t i=0; i<=10; i++) {
|
|
update_notify();
|
|
}
|
|
|
|
#if LOGGING_ENABLED == ENABLED
|
|
// start dataflash
|
|
start_logging();
|
|
#endif
|
|
|
|
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
|
gcs_send_text_P(SEVERITY_HIGH, PSTR("ARMING MOTORS"));
|
|
#endif
|
|
|
|
// Remember Orientation
|
|
// --------------------
|
|
init_simple_bearing();
|
|
|
|
initial_armed_bearing = ahrs.yaw_sensor;
|
|
|
|
// Reset home position
|
|
// -------------------
|
|
if (ap.home_is_set) {
|
|
init_home();
|
|
calc_distance_and_bearing();
|
|
}
|
|
|
|
if(did_ground_start == false) {
|
|
did_ground_start = true;
|
|
startup_ground(true);
|
|
}
|
|
|
|
// fast baro calibration to reset ground pressure
|
|
init_barometer(false);
|
|
|
|
// reset inertial nav alt to zero
|
|
inertial_nav.set_altitude(0.0f);
|
|
|
|
// go back to normal AHRS gains
|
|
ahrs.set_fast_gains(false);
|
|
|
|
// enable gps velocity based centrefugal force compensation
|
|
ahrs.set_correct_centrifugal(true);
|
|
ahrs.set_armed(true);
|
|
|
|
// set hover throttle
|
|
motors.set_mid_throttle(g.throttle_mid);
|
|
|
|
// Cancel arming if throttle is raised too high so that copter does not suddenly take off
|
|
read_radio();
|
|
if (g.rc_3.control_in > g.throttle_cruise && g.throttle_cruise > 100) {
|
|
motors.output_min();
|
|
failsafe_enable();
|
|
AP_Notify::flags.armed = false;
|
|
AP_Notify::flags.arming_failed = false;
|
|
return;
|
|
}
|
|
|
|
#if SPRAYER == ENABLED
|
|
// turn off sprayer's test if on
|
|
sprayer.test_pump(false);
|
|
#endif
|
|
|
|
// enable output to motors
|
|
output_min();
|
|
|
|
// finally actually arm the motors
|
|
motors.armed(true);
|
|
|
|
// log arming to dataflash
|
|
Log_Write_Event(DATA_ARMED);
|
|
|
|
// reenable failsafe
|
|
failsafe_enable();
|
|
}
|
|
|
|
// perform pre-arm checks and set ap.pre_arm_check flag
|
|
static void pre_arm_checks(bool display_failure)
|
|
{
|
|
// exit immediately if we've already successfully performed the pre-arm check
|
|
if (ap.pre_arm_check) {
|
|
return;
|
|
}
|
|
|
|
// succeed if pre arm checks are disabled
|
|
if(g.arming_check == ARMING_CHECK_NONE) {
|
|
set_pre_arm_check(true);
|
|
set_pre_arm_rc_check(true);
|
|
return;
|
|
}
|
|
|
|
// pre-arm rc checks a prerequisite
|
|
pre_arm_rc_checks();
|
|
if(!ap.pre_arm_rc_check) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: RC not calibrated"));
|
|
}
|
|
return;
|
|
}
|
|
|
|
// check Baro
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
|
|
// barometer health check
|
|
if(!barometer.healthy) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Baro not healthy"));
|
|
}
|
|
return;
|
|
}
|
|
// check Baro & inav alt are within 1m
|
|
if(fabs(inertial_nav.get_altitude() - baro_alt) > 100) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Alt disparity"));
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
// check Compass
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) {
|
|
// check the primary compass is healthy
|
|
if(!compass.healthy(0)) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not healthy"));
|
|
}
|
|
return;
|
|
}
|
|
|
|
// check compass learning is on or offsets have been set
|
|
if(!compass.learn_offsets_enabled() && !compass.configured()) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass not calibrated"));
|
|
}
|
|
return;
|
|
}
|
|
|
|
// check for unreasonable compass offsets
|
|
Vector3f offsets = compass.get_offsets();
|
|
if(offsets.length() > 500) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Compass offsets too high"));
|
|
}
|
|
return;
|
|
}
|
|
|
|
// check for unreasonable mag field length
|
|
float mag_field = compass.get_field().length();
|
|
if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65 || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check mag field"));
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
// check GPS
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_GPS)) {
|
|
// check gps is ok if required - note this same check is repeated again in arm_checks
|
|
if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
|
|
return;
|
|
}
|
|
|
|
#if AC_FENCE == ENABLED
|
|
// check fence is initialised
|
|
if(!fence.pre_arm_check() || (((fence.get_enabled_fences() & AC_FENCE_TYPE_CIRCLE) != 0) && !pre_arm_gps_checks(display_failure))) {
|
|
return;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
// check INS
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
|
|
// check accelerometers have been calibrated
|
|
if(!ins.calibrated()) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not calibrated"));
|
|
}
|
|
return;
|
|
}
|
|
|
|
// check accels and gyros are healthy
|
|
if(!ins.healthy()) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: INS not healthy"));
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
#if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
|
|
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
|
|
// check board voltage
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
|
|
if(hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Board Voltage"));
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
#endif
|
|
#endif
|
|
|
|
// check various parameter values
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
|
|
|
|
// ensure ch7 and ch8 have different functions
|
|
if ((g.ch7_option != 0 || g.ch8_option != 0) && g.ch7_option == g.ch8_option) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Ch7&Ch8 Opt cannot be same"));
|
|
}
|
|
return;
|
|
}
|
|
|
|
// failsafe parameter checks
|
|
if (g.failsafe_throttle) {
|
|
// check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
|
|
if (g.rc_3.radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check FS_THR_VALUE"));
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
// lean angle parameter check
|
|
if (aparm.angle_max < 1000 || aparm.angle_max > 8000) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check ANGLE_MAX"));
|
|
}
|
|
return;
|
|
}
|
|
|
|
// acro balance parameter check
|
|
if ((g.acro_balance_roll > g.p_stabilize_roll.kP()) || (g.acro_balance_pitch > g.p_stabilize_pitch.kP())) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: ACRO_BAL_ROLL/PITCH"));
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
// if we've gotten this far then pre arm checks have completed
|
|
set_pre_arm_check(true);
|
|
}
|
|
|
|
// perform pre_arm_rc_checks checks and set ap.pre_arm_rc_check flag
|
|
static void pre_arm_rc_checks()
|
|
{
|
|
// exit immediately if we've already successfully performed the pre-arm rc check
|
|
if( ap.pre_arm_rc_check ) {
|
|
return;
|
|
}
|
|
|
|
// set rc-checks to success if RC checks are disabled
|
|
if ((g.arming_check != ARMING_CHECK_ALL) && !(g.arming_check & ARMING_CHECK_RC)) {
|
|
set_pre_arm_rc_check(true);
|
|
return;
|
|
}
|
|
|
|
// check if radio has been calibrated
|
|
if(!g.rc_3.radio_min.load() && !g.rc_3.radio_max.load()) {
|
|
return;
|
|
}
|
|
|
|
// check channels 1 & 2 have min <= 1300 and max >= 1700
|
|
if (g.rc_1.radio_min > 1300 || g.rc_1.radio_max < 1700 || g.rc_2.radio_min > 1300 || g.rc_2.radio_max < 1700) {
|
|
return;
|
|
}
|
|
|
|
// check channels 3 & 4 have min <= 1300 and max >= 1700
|
|
if (g.rc_3.radio_min > 1300 || g.rc_3.radio_max < 1700 || g.rc_4.radio_min > 1300 || g.rc_4.radio_max < 1700) {
|
|
return;
|
|
}
|
|
|
|
// if we've gotten this far rc is ok
|
|
set_pre_arm_rc_check(true);
|
|
}
|
|
|
|
// performs pre_arm gps related checks and returns true if passed
|
|
static bool pre_arm_gps_checks(bool display_failure)
|
|
{
|
|
float speed_cms = inertial_nav.get_velocity().length(); // speed according to inertial nav in cm/s
|
|
|
|
// check GPS is not glitching
|
|
if (gps_glitch.glitching()) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: GPS Glitch"));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// ensure GPS is ok
|
|
if (!GPS_ok()) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Need 3D Fix"));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// check speed is below 50cm/s
|
|
if (speed_cms == 0 || speed_cms > PREARM_MAX_VELOCITY_CMS) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Bad Velocity"));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// warn about hdop separately - to prevent user confusion with no gps lock
|
|
if (gps.get_hdop() > g.gps_hdop_good) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: High GPS HDOP"));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// if we got here all must be ok
|
|
return true;
|
|
}
|
|
|
|
// arm_checks - perform final checks before arming
|
|
// always called just before arming. Return true if ok to arm
|
|
static bool arm_checks(bool display_failure)
|
|
{
|
|
// succeed if arming checks are disabled
|
|
if (g.arming_check == ARMING_CHECK_NONE) {
|
|
return true;
|
|
}
|
|
|
|
// check Baro & inav alt are within 1m
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
|
|
if(fabs(inertial_nav.get_altitude() - baro_alt) > 100) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Alt disparity"));
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// check gps is ok if required - note this same check is also done in pre-arm checks
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_GPS)) {
|
|
if ((mode_requires_GPS(control_mode) || g.failsafe_gps_enabled == FS_GPS_LAND_EVEN_STABILIZE) && !pre_arm_gps_checks(display_failure)) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// check parameters
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {
|
|
// check throttle is above failsafe throttle
|
|
if (g.failsafe_throttle != FS_THR_DISABLED && g.rc_3.radio_in < g.failsafe_throttle_value) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Thr below FS"));
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// check lean angle
|
|
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
|
|
if (labs(ahrs.roll_sensor) > aparm.angle_max || labs(ahrs.pitch_sensor) > aparm.angle_max) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Leaning"));
|
|
}
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// check if safety switch has been pushed
|
|
if (hal.util->safety_switch_state() == AP_HAL::Util::SAFETY_DISARMED) {
|
|
if (display_failure) {
|
|
gcs_send_text_P(SEVERITY_HIGH,PSTR("Arm: Safety Switch"));
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// if we've gotten this far all is ok
|
|
return true;
|
|
}
|
|
|
|
// init_disarm_motors - disarm motors
|
|
static void init_disarm_motors()
|
|
{
|
|
// return immediately if we are already disarmed
|
|
if (!motors.armed()) {
|
|
return;
|
|
}
|
|
|
|
#if HIL_MODE != HIL_MODE_DISABLED || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
|
|
gcs_send_text_P(SEVERITY_HIGH, PSTR("DISARMING MOTORS"));
|
|
#endif
|
|
|
|
motors.armed(false);
|
|
|
|
// disable inertial nav errors temporarily
|
|
inertial_nav.ignore_next_error();
|
|
|
|
// save offsets if automatic offset learning is on
|
|
if (compass.learn_offsets_enabled()) {
|
|
compass.save_offsets();
|
|
}
|
|
|
|
g.throttle_cruise.save();
|
|
|
|
#if AUTOTUNE_ENABLED == ENABLED
|
|
// save auto tuned parameters
|
|
autotune_save_tuning_gains();
|
|
#endif
|
|
|
|
// we are not in the air
|
|
set_land_complete(true);
|
|
|
|
// reset the mission
|
|
mission.reset();
|
|
|
|
// setup fast AHRS gains to get right attitude
|
|
ahrs.set_fast_gains(true);
|
|
|
|
// log disarm to the dataflash
|
|
Log_Write_Event(DATA_DISARMED);
|
|
|
|
// suspend logging
|
|
DataFlash.EnableWrites(false);
|
|
|
|
// disable gps velocity based centrefugal force compensation
|
|
ahrs.set_correct_centrifugal(false);
|
|
ahrs.set_armed(false);
|
|
}
|
|
|
|
/*****************************************
|
|
* Set the flight control servos based on the current calculated values
|
|
*****************************************/
|
|
static void
|
|
set_servos_4()
|
|
{
|
|
// check if we are performing the motor test
|
|
if (ap.motor_test) {
|
|
motor_test_output();
|
|
} else {
|
|
#if FRAME_CONFIG == TRI_FRAME
|
|
// To-Do: implement improved stability patch for tri so that we do not need to limit throttle input to motors
|
|
g.rc_3.servo_out = min(g.rc_3.servo_out, 800);
|
|
#endif
|
|
motors.output();
|
|
}
|
|
}
|