2013-11-27 22:18:14 -04:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
2015-08-11 03:28:42 -03:00
# include "AP_Arming.h"
# include <AP_Notify/AP_Notify.h>
2015-09-08 00:54:25 -03:00
# include <GCS_MAVLink/GCS.h>
2013-11-27 22:18:14 -04:00
2016-08-17 05:15:38 -03:00
# define AP_ARMING_COMPASS_MAGFIELD_EXPECTED 530
2015-10-16 00:09:39 -03:00
# define AP_ARMING_COMPASS_MAGFIELD_MIN 185 // 0.35 * 530 milligauss
# define AP_ARMING_COMPASS_MAGFIELD_MAX 875 // 1.65 * 530 milligauss
2015-10-16 00:13:53 -03:00
# define AP_ARMING_BOARD_VOLTAGE_MIN 4.3f
# define AP_ARMING_BOARD_VOLTAGE_MAX 5.8f
2015-12-21 17:51:12 -04:00
# define AP_ARMING_ACCEL_ERROR_THRESHOLD 0.75f
2017-09-18 00:14:52 -03:00
# define AP_ARMING_AHRS_GPS_ERROR_MAX 10 // accept up to 10m difference between AHRS and GPS
2015-10-16 00:13:53 -03:00
2013-11-27 22:18:14 -04:00
extern const AP_HAL : : HAL & hal ;
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_Arming : : var_info [ ] = {
2016-12-28 01:41:21 -04:00
2013-11-27 22:18:14 -04:00
// @Param: REQUIRE
// @DisplayName: Require Arming Motors
2017-02-14 15:31:23 -04:00
// @Description: Arming disabled until some requirements are met. If 0, there are no requirements (arm immediately). If 1, require rudder stick or GCS arming before arming motors and sends the minimum throttle PWM value to the throttle channel when disarmed. If 2, require rudder stick or GCS arming and send 0 PWM to throttle channel when disarmed. See the ARMING_CHECK_* parameters to see what checks are done before arming. Note, if setting this parameter to 0 a reboot is required to arm the plane. Also note, even with this parameter at 0, if ARMING_CHECK parameter is not also zero the plane may fail to arm throttle at boot due to a pre-arm check failure. This parameter is relevant for ArduPlane only.
2014-01-09 15:34:58 -04:00
// @Values: 0:Disabled,1:THR_MIN PWM when disarmed,2:0 PWM when disarmed
2013-11-27 22:18:14 -04:00
// @User: Advanced
2017-02-07 19:47:01 -04:00
AP_GROUPINFO_FLAGS_FRAME ( " REQUIRE " , 0 , AP_Arming , require , 1 ,
AP_PARAM_NO_SHIFT ,
AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_ROVER ) ,
2013-11-27 22:18:14 -04:00
// @Param: CHECK
// @DisplayName: Arm Checks to Peform (bitmask)
2017-05-16 07:23:21 -03:00
// @Description: Checks prior to arming motor. This is a bitmask of checks that will be performed before allowing arming. The default is no checks, allowing arming at any time. You can select whatever checks you prefer by adding together the values of each check type to set this parameter. For example, to only allow arming when you have GPS lock and no RC failsafe you would set ARMING_CHECK to 72. For most users it is recommended that you set this to 1 to enable all checks.
2017-09-23 08:10:10 -03:00
// @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Failsafe,128:Board voltage,256:Battery Level,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration
// @Values{Plane}: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Failsafe,128:Board voltage,256:Battery Level,512:Airspeed,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration
// @Bitmask: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration
// @Bitmask{Plane}: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC,7:Board voltage,8:Battery Level,9:Airspeed,10:Logging Available,11:Hardware safety switch,12:GPS Configuration
2015-10-15 03:26:00 -03:00
// @User: Standard
2015-01-19 20:27:13 -04:00
AP_GROUPINFO ( " CHECK " , 2 , AP_Arming , checks_to_perform , ARMING_CHECK_ALL ) ,
2013-11-27 22:18:14 -04:00
2015-12-21 17:51:12 -04:00
// @Param: ACCTHRESH
// @DisplayName: Accelerometer error threshold
// @Description: Accelerometer error threshold used to determine inconsistent accelerometers. Compares this error range to other accelerometers to detect a hardware or calibration error. Lower value means tighter check and harder to pass arming check. Not all accelerometers are created equal.
// @Units: m/s/s
// @Range: 0.25 3.0
// @User: Advanced
AP_GROUPINFO ( " ACCTHRESH " , 3 , AP_Arming , accel_error_threshold , AP_ARMING_ACCEL_ERROR_THRESHOLD ) ,
2017-07-26 07:20:02 -03:00
// @Param: VOLT_MIN
// @DisplayName: Arming voltage minimum on the first battery
2017-10-29 20:13:49 -03:00
// @Description: The minimum voltage of the first battery required to arm, 0 disables the check
2017-05-02 10:40:34 -03:00
// @Units: V
2016-02-09 19:31:12 -04:00
// @Increment: 0.1
// @User: Standard
2017-07-26 07:20:02 -03:00
AP_GROUPINFO ( " VOLT_MIN " , 4 , AP_Arming , _min_voltage [ 0 ] , 0 ) ,
2016-02-09 19:31:12 -04:00
2017-07-26 07:20:02 -03:00
// @Param: VOLT2_MIN
// @DisplayName: Arming voltage minimum on the second battery
2017-10-29 20:13:49 -03:00
// @Description: The minimum voltage of the second battery required to arm, 0 disables the check
2017-05-02 10:40:34 -03:00
// @Units: V
2016-02-09 19:31:12 -04:00
// @Increment: 0.1
// @User: Standard
2017-07-26 07:20:02 -03:00
AP_GROUPINFO ( " VOLT2_MIN " , 5 , AP_Arming , _min_voltage [ 1 ] , 0 ) ,
2016-02-09 19:31:12 -04:00
2013-11-27 22:18:14 -04:00
AP_GROUPEND
} ;
//The function point is particularly hacky, hacky, tacky
//but I don't want to reimplement messaging to GCS at the moment:
2018-03-05 16:35:06 -04:00
AP_Arming : : AP_Arming ( const AP_AHRS & ahrs_ref , Compass & compass ,
2016-08-15 00:37:03 -03:00
const AP_BattMonitor & battery ) :
2015-10-12 02:29:27 -03:00
ahrs ( ahrs_ref ) ,
_compass ( compass ) ,
2016-02-09 19:31:12 -04:00
_battery ( battery ) ,
2015-10-12 02:29:27 -03:00
armed ( false ) ,
arming_method ( NONE )
2013-11-27 22:18:14 -04:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2016-12-28 01:41:21 -04:00
# if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
// default REQUIRE parameter to 1 (needed for Copter which is missing the parameter declaration above)
require . set_default ( YES_MIN_PWM ) ;
# endif
2015-05-08 07:23:21 -03:00
memset ( last_accel_pass_ms , 0 , sizeof ( last_accel_pass_ms ) ) ;
memset ( last_gyro_pass_ms , 0 , sizeof ( last_gyro_pass_ms ) ) ;
2013-11-27 22:18:14 -04:00
}
2016-08-17 05:15:38 -03:00
uint16_t AP_Arming : : compass_magfield_expected ( ) const
{
return AP_ARMING_COMPASS_MAGFIELD_EXPECTED ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : is_armed ( )
2016-12-28 01:41:21 -04:00
{
return require = = NONE | | armed ;
2013-12-11 02:02:25 -04:00
}
2013-11-27 22:18:14 -04:00
2015-10-12 02:29:27 -03:00
uint16_t AP_Arming : : get_enabled_checks ( )
2013-12-11 02:02:25 -04:00
{
2013-11-27 22:18:14 -04:00
return checks_to_perform ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : barometer_checks ( bool report )
2013-12-11 02:02:25 -04:00
{
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | |
( checks_to_perform & ARMING_CHECK_BARO ) ) {
2018-03-05 16:35:06 -04:00
if ( ! AP : : baro ( ) . all_healthy ( ) ) {
2014-01-23 06:39:55 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Barometer not healthy " ) ;
2014-01-23 06:39:55 -04:00
}
2013-11-27 22:18:14 -04:00
return false ;
}
}
return true ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : airspeed_checks ( bool report )
2015-01-19 20:27:13 -04:00
{
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | |
( checks_to_perform & ARMING_CHECK_AIRSPEED ) ) {
const AP_Airspeed * airspeed = ahrs . get_airspeed ( ) ;
2016-10-30 02:24:21 -03:00
if ( airspeed = = nullptr ) {
2015-01-19 20:27:13 -04:00
// not an airspeed capable vehicle
return true ;
}
2017-11-03 05:26:02 -03:00
for ( uint8_t i = 0 ; i < AIRSPEED_MAX_SENSORS ; i + + ) {
if ( airspeed - > enabled ( i ) & & airspeed - > use ( i ) & & ! airspeed - > healthy ( i ) ) {
if ( report ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Airspeed[%u] not healthy " , i ) ;
}
return false ;
2015-01-19 20:27:13 -04:00
}
}
}
return true ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : logging_checks ( bool report )
2015-02-06 03:38:53 -04:00
{
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | |
( checks_to_perform & ARMING_CHECK_LOGGING ) ) {
2016-11-05 19:05:36 -03:00
if ( DataFlash_Class : : instance ( ) - > logging_failed ( ) ) {
2015-02-06 03:38:53 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Logging failed " ) ;
2016-11-05 19:05:36 -03:00
}
2016-11-05 20:23:14 -03:00
return false ;
2016-11-05 19:05:36 -03:00
}
if ( ! DataFlash_Class : : instance ( ) - > CardInserted ( ) ) {
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: No SD card " ) ;
2015-02-06 03:38:53 -04:00
}
return false ;
}
}
return true ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : ins_checks ( bool report )
2015-01-19 20:27:13 -04:00
{
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | |
( checks_to_perform & ARMING_CHECK_INS ) ) {
2018-03-10 05:35:13 -04:00
const AP_InertialSensor & ins = AP : : ins ( ) ;
2015-10-12 02:29:27 -03:00
if ( ! ins . get_gyro_health_all ( ) ) {
2015-01-19 20:27:13 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Gyros not healthy " ) ;
2015-01-20 04:47:15 -04:00
}
return false ;
}
2015-09-17 09:33:41 -03:00
if ( ! ins . gyro_calibrated_ok_all ( ) ) {
2015-01-20 04:47:15 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Gyros not calibrated " ) ;
2015-01-20 04:47:15 -04:00
}
return false ;
}
2015-10-12 02:29:27 -03:00
if ( ! ins . get_accel_health_all ( ) ) {
2015-01-20 04:47:15 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Accels not healthy " ) ;
2015-01-19 20:27:13 -04:00
}
return false ;
}
2015-05-12 03:19:29 -03:00
if ( ! ins . accel_calibrated_ok_all ( ) ) {
2015-01-31 22:31:59 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: 3D Accel calibration needed " ) ;
2015-01-31 22:31:59 -04:00
}
return false ;
}
2015-12-29 00:33:40 -04:00
//check if accelerometers have calibrated and require reboot
if ( ins . accel_cal_requires_reboot ( ) ) {
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Accels calibrated requires reboot " ) ;
2015-12-29 00:33:40 -04:00
}
return false ;
}
2015-10-14 12:44:35 -03:00
2015-01-19 20:27:13 -04:00
// check all accelerometers point in roughly same direction
if ( ins . get_accel_count ( ) > 1 ) {
const Vector3f & prime_accel_vec = ins . get_accel ( ) ;
for ( uint8_t i = 0 ; i < ins . get_accel_count ( ) ; i + + ) {
2016-01-19 03:05:08 -04:00
if ( ! ins . use_accel ( i ) ) {
continue ;
}
2015-01-19 20:27:13 -04:00
// get next accel vector
const Vector3f & accel_vec = ins . get_accel ( i ) ;
Vector3f vec_diff = accel_vec - prime_accel_vec ;
2015-12-21 17:51:12 -04:00
// allow for user-defined difference, typically 0.75 m/s/s. Has to pass in last 10 seconds
float threshold = accel_error_threshold ;
2015-05-08 07:23:21 -03:00
if ( i > = 2 ) {
/*
we allow for a higher threshold for IMU3 as it
runs at a different temperature to IMU1 / IMU2 ,
and is not used for accel data in the EKF
*/
threshold * = 3 ;
2015-03-31 19:29:50 -03:00
}
2016-05-16 17:45:17 -03:00
2016-05-17 20:31:42 -03:00
// EKF is less sensitive to Z-axis error
2016-04-18 17:36:17 -03:00
vec_diff . z * = 0.5f ;
2016-05-16 17:45:17 -03:00
2015-05-08 07:23:21 -03:00
if ( vec_diff . length ( ) < = threshold ) {
2015-11-19 23:07:23 -04:00
last_accel_pass_ms [ i ] = AP_HAL : : millis ( ) ;
2015-05-08 07:23:21 -03:00
}
2015-11-19 23:07:23 -04:00
if ( AP_HAL : : millis ( ) - last_accel_pass_ms [ i ] > 10000 ) {
2015-01-19 20:27:13 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Accels inconsistent " ) ;
2015-01-19 20:27:13 -04:00
}
return false ;
}
}
}
// check all gyros are giving consistent readings
if ( ins . get_gyro_count ( ) > 1 ) {
const Vector3f & prime_gyro_vec = ins . get_gyro ( ) ;
for ( uint8_t i = 0 ; i < ins . get_gyro_count ( ) ; i + + ) {
2016-01-19 03:05:08 -04:00
if ( ! ins . use_gyro ( i ) ) {
continue ;
}
2015-01-19 20:27:13 -04:00
// get next gyro vector
const Vector3f & gyro_vec = ins . get_gyro ( i ) ;
Vector3f vec_diff = gyro_vec - prime_gyro_vec ;
2016-09-16 22:43:32 -03:00
// allow for up to 5 degrees/s difference. Pass if it has
2015-03-31 19:29:50 -03:00
// been OK in last 10 seconds
if ( vec_diff . length ( ) < = radians ( 5 ) ) {
2015-11-19 23:07:23 -04:00
last_gyro_pass_ms [ i ] = AP_HAL : : millis ( ) ;
2015-03-31 19:29:50 -03:00
}
2015-11-19 23:07:23 -04:00
if ( AP_HAL : : millis ( ) - last_gyro_pass_ms [ i ] > 10000 ) {
2015-01-19 20:27:13 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Gyros inconsistent " ) ;
2015-01-19 20:27:13 -04:00
}
return false ;
}
}
}
}
return true ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : compass_checks ( bool report )
2013-12-11 02:02:25 -04:00
{
if ( ( checks_to_perform ) & ARMING_CHECK_ALL | |
( checks_to_perform ) & ARMING_CHECK_COMPASS ) {
2013-11-27 22:18:14 -04:00
2015-01-19 20:33:39 -04:00
if ( ! _compass . use_for_yaw ( ) ) {
// compass use is disabled
return true ;
}
2014-07-10 10:47:59 -03:00
if ( ! _compass . healthy ( ) ) {
2014-01-23 06:39:55 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Compass not healthy " ) ;
2014-01-23 06:39:55 -04:00
}
2013-11-27 22:18:14 -04:00
return false ;
}
// check compass learning is on or offsets have been set
2014-07-10 10:47:59 -03:00
if ( ! _compass . learn_offsets_enabled ( ) & & ! _compass . configured ( ) ) {
2014-01-23 06:39:55 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Compass not calibrated " ) ;
2014-01-23 06:39:55 -04:00
}
2013-11-27 22:18:14 -04:00
return false ;
}
2015-01-19 20:27:13 -04:00
2015-08-27 23:44:33 -03:00
//check if compass is calibrating
2015-10-12 02:29:27 -03:00
if ( _compass . is_calibrating ( ) ) {
2015-08-27 23:44:33 -03:00
if ( report ) {
2017-07-20 21:20:29 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Compass calibration running " ) ;
2015-08-27 23:44:33 -03:00
}
return false ;
}
//check if compass has calibrated and requires reboot
2015-10-12 02:29:27 -03:00
if ( _compass . compass_cal_requires_reboot ( ) ) {
2015-08-27 23:44:33 -03:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Compass calibrated requires reboot " ) ;
2015-08-27 23:44:33 -03:00
}
return false ;
}
2015-01-19 20:27:13 -04:00
// check for unreasonable compass offsets
2015-09-28 16:56:17 -03:00
Vector3f offsets = _compass . get_offsets ( ) ;
2017-04-03 19:38:36 -03:00
if ( offsets . length ( ) > _compass . get_offsets_max ( ) ) {
2015-01-19 20:27:13 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Compass offsets too high " ) ;
2015-01-19 20:27:13 -04:00
}
return false ;
}
// check for unreasonable mag field length
2015-09-28 16:56:17 -03:00
float mag_field = _compass . get_field ( ) . length ( ) ;
2015-10-16 00:09:39 -03:00
if ( mag_field > AP_ARMING_COMPASS_MAGFIELD_MAX | | mag_field < AP_ARMING_COMPASS_MAGFIELD_MIN ) {
2015-01-19 20:27:13 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Check mag field " ) ;
2015-01-19 20:27:13 -04:00
}
return false ;
}
2015-10-16 00:09:39 -03:00
// check all compasses point in roughly same direction
if ( ! _compass . consistent ( ) ) {
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Compasses inconsistent " ) ;
2017-05-16 07:23:21 -03:00
}
2015-10-16 00:09:39 -03:00
return false ;
}
2013-11-27 22:18:14 -04:00
}
return true ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : gps_checks ( bool report )
2013-12-11 02:02:25 -04:00
{
2017-10-25 00:59:05 -03:00
const AP_GPS & gps = AP : : gps ( ) ;
2016-02-02 03:45:41 -04:00
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | | ( checks_to_perform & ARMING_CHECK_GPS ) ) {
2013-11-27 22:18:14 -04:00
//GPS OK?
2018-03-15 22:06:04 -03:00
if ( ! AP : : ahrs ( ) . home_is_set ( ) | |
2015-03-12 23:03:04 -03:00
gps . status ( ) < AP_GPS : : GPS_OK_FIX_3D ) {
2014-01-23 06:39:55 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Bad GPS Position " ) ;
2014-01-23 06:39:55 -04:00
}
2013-11-27 22:18:14 -04:00
return false ;
2015-10-12 02:29:27 -03:00
}
2017-07-08 20:07:24 -03:00
//GPS update rate acceptable
if ( ! gps . is_healthy ( ) ) {
if ( report ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: GPS is not healthy " ) ;
}
return false ;
}
2017-03-08 20:47:57 -04:00
2017-08-29 17:33:33 -03:00
// check GPSs are within 50m of each other and that blending is healthy
2017-03-08 20:47:57 -04:00
float distance_m ;
if ( ! gps . all_consistent ( distance_m ) ) {
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL ,
2017-03-10 02:38:04 -04:00
" PreArm: GPS positions differ by %4.1fm " ,
( double ) distance_m ) ;
2017-03-08 20:47:57 -04:00
}
return false ;
}
2017-03-10 04:12:46 -04:00
if ( ! gps . blend_health_check ( ) ) {
2017-03-08 20:47:57 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: GPS blending unhealthy " ) ;
2017-03-08 20:47:57 -04:00
}
return false ;
}
2017-09-18 00:14:52 -03:00
// check AHRS and GPS are within 10m of each other
2017-10-25 00:59:05 -03:00
Location gps_loc = gps . location ( ) ;
2017-09-18 00:14:52 -03:00
Location ahrs_loc ;
if ( ahrs . get_position ( ahrs_loc ) ) {
float distance = location_3d_diff_NED ( gps_loc , ahrs_loc ) . length ( ) ;
if ( distance > AP_ARMING_AHRS_GPS_ERROR_MAX ) {
if ( report ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: GPS and AHRS differ by %4.1fm " , ( double ) distance ) ;
}
return false ;
}
}
2017-03-08 20:47:57 -04:00
}
2017-08-29 17:33:33 -03:00
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | | ( checks_to_perform & ARMING_CHECK_GPS_CONFIG ) ) {
uint8_t first_unconfigured = gps . first_unconfigured_gps ( ) ;
if ( first_unconfigured ! = AP_GPS : : GPS_ALL_CONFIGURED ) {
if ( report ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL ,
" PreArm: GPS %d failing configuration checks " ,
first_unconfigured + 1 ) ;
gps . broadcast_first_configuration_failure_reason ( ) ;
}
return false ;
}
}
2013-11-27 22:18:14 -04:00
return true ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : battery_checks ( bool report )
2013-12-11 02:02:25 -04:00
{
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | |
( checks_to_perform & ARMING_CHECK_BATTERY ) ) {
2013-11-27 22:18:14 -04:00
if ( AP_Notify : : flags . failsafe_battery ) {
2014-01-23 06:39:55 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Battery failsafe on " ) ;
2014-01-23 06:39:55 -04:00
}
2013-11-27 22:18:14 -04:00
return false ;
}
2017-07-26 07:56:31 -03:00
for ( uint8_t i = 0 ; i < _battery . num_instances ( ) ; i + + ) {
2016-02-09 19:31:12 -04:00
if ( ( _min_voltage [ i ] > 0.0f ) & & ( _battery . voltage ( i ) < _min_voltage [ i ] ) ) {
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Battery %d voltage %.1f below minimum %.1f " ,
2016-03-14 13:04:52 -03:00
i + 1 ,
( double ) _battery . voltage ( i ) ,
( double ) _min_voltage [ i ] ) ;
2016-02-09 19:31:12 -04:00
}
return false ;
}
}
}
2013-11-27 22:18:14 -04:00
return true ;
}
2014-01-23 06:39:55 -04:00
bool AP_Arming : : hardware_safety_check ( bool report )
2013-12-11 02:02:25 -04:00
{
2015-12-21 02:34:00 -04:00
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | |
( checks_to_perform & ARMING_CHECK_SWITCH ) ) {
// check if safety switch has been pushed
if ( hal . util - > safety_switch_state ( ) = = AP_HAL : : Util : : SAFETY_DISARMED ) {
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Hardware safety switch " ) ;
2015-12-21 02:34:00 -04:00
}
return false ;
}
2013-11-27 22:18:14 -04:00
}
return true ;
}
2017-09-04 01:16:12 -03:00
bool AP_Arming : : rc_calibration_checks ( bool report )
{
return true ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : manual_transmitter_checks ( bool report )
2013-12-11 02:02:25 -04:00
{
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | |
( checks_to_perform & ARMING_CHECK_RC ) ) {
2013-11-27 22:18:14 -04:00
if ( AP_Notify : : flags . failsafe_radio ) {
2014-01-23 06:39:55 -04:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Radio failsafe on " ) ;
2014-01-23 06:39:55 -04:00
}
2013-11-27 22:18:14 -04:00
return false ;
}
2017-09-04 01:16:12 -03:00
if ( ! rc_calibration_checks ( report ) ) {
return false ;
}
2013-11-27 22:18:14 -04:00
}
return true ;
}
2015-10-16 00:13:53 -03:00
bool AP_Arming : : board_voltage_checks ( bool report )
{
2017-07-15 04:38:37 -03:00
# if HAL_HAVE_BOARD_VOLTAGE
2015-10-16 00:13:53 -03:00
// check board voltage
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | | ( checks_to_perform & ARMING_CHECK_VOLTAGE ) ) {
2017-07-15 04:38:37 -03:00
if ( ( ( hal . analogin - > board_voltage ( ) < AP_ARMING_BOARD_VOLTAGE_MIN ) | | ( hal . analogin - > board_voltage ( ) > AP_ARMING_BOARD_VOLTAGE_MAX ) ) ) {
2015-10-16 00:13:53 -03:00
if ( report ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: Check board voltage " ) ;
2015-10-16 00:13:53 -03:00
}
return false ;
}
}
2017-07-15 04:38:37 -03:00
# endif
2015-10-16 00:13:53 -03:00
return true ;
}
2015-10-12 02:29:27 -03:00
bool AP_Arming : : pre_arm_checks ( bool report )
2013-12-11 02:02:25 -04:00
{
2016-12-28 01:41:21 -04:00
# if !APM_BUILD_TYPE(APM_BUILD_ArduCopter)
2014-01-23 06:39:55 -04:00
if ( armed | | require = = NONE ) {
// if we are already armed or don't need any arming checks
// then skip the checks
return true ;
}
2016-12-28 01:41:21 -04:00
# endif
2014-01-23 06:39:55 -04:00
2016-12-03 08:30:46 -04:00
return hardware_safety_check ( report )
& barometer_checks ( report )
& ins_checks ( report )
& compass_checks ( report )
& gps_checks ( report )
& battery_checks ( report )
& logging_checks ( report )
& manual_transmitter_checks ( report )
& board_voltage_checks ( report ) ;
2013-11-27 22:18:14 -04:00
}
2016-11-05 20:23:14 -03:00
bool AP_Arming : : arm_checks ( uint8_t method )
{
2017-10-19 22:16:37 -03:00
// ensure the GPS drivers are ready on any final changes
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | |
( checks_to_perform & ARMING_CHECK_GPS_CONFIG ) ) {
2017-10-25 00:59:05 -03:00
if ( ! AP : : gps ( ) . prepare_for_arming ( ) ) {
2017-10-19 22:16:37 -03:00
return false ;
}
}
2017-06-30 09:31:08 -03:00
// note that this will prepare DataFlash to start logging
// so should be the last check to be done before arming
2016-11-05 20:23:14 -03:00
if ( ( checks_to_perform & ARMING_CHECK_ALL ) | |
( checks_to_perform & ARMING_CHECK_LOGGING ) ) {
2017-06-30 09:31:08 -03:00
DataFlash_Class * df = DataFlash_Class : : instance ( ) ;
df - > PrepForArming ( ) ;
if ( ! df - > logging_started ( ) ) {
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " Arm: Logging not started " ) ;
2016-11-05 20:23:14 -03:00
return false ;
}
}
return true ;
}
2016-05-12 13:51:51 -03:00
//returns true if arming occurred successfully
2015-10-12 02:29:27 -03:00
bool AP_Arming : : arm ( uint8_t method )
2013-12-11 02:02:25 -04:00
{
2016-12-28 01:41:21 -04:00
# if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
// Copter should never use this function
return false ;
# else
2013-11-27 22:18:14 -04:00
if ( armed ) { //already armed
return false ;
}
//are arming checks disabled?
if ( checks_to_perform = = ARMING_CHECK_NONE ) {
armed = true ;
arming_method = NONE ;
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Throttle armed " ) ;
2013-11-27 22:18:14 -04:00
return true ;
}
2016-11-05 20:23:14 -03:00
if ( pre_arm_checks ( true ) & & arm_checks ( method ) ) {
2013-11-27 22:18:14 -04:00
armed = true ;
arming_method = method ;
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Throttle armed " ) ;
2013-11-27 22:18:14 -04:00
//TODO: Log motor arming to the dataflash
//Can't do this from this class until there is a unified logging library
} else {
armed = false ;
arming_method = NONE ;
}
return armed ;
2016-12-28 01:41:21 -04:00
# endif
2013-11-27 22:18:14 -04:00
}
//returns true if disarming occurred successfully
2013-12-11 02:02:25 -04:00
bool AP_Arming : : disarm ( )
{
2016-12-28 01:41:21 -04:00
# if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
// Copter should never use this function
return false ;
# else
2015-10-12 02:29:27 -03:00
if ( ! armed ) { // already disarmed
2013-11-27 22:18:14 -04:00
return false ;
}
armed = false ;
2017-07-09 01:06:18 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Throttle disarmed " ) ;
2013-11-27 22:18:14 -04:00
//TODO: Log motor disarming to the dataflash
//Can't do this from this class until there is a unified logging library.
return true ;
2016-12-28 01:41:21 -04:00
# endif
2013-11-27 22:18:14 -04:00
}
2013-12-11 02:02:25 -04:00
AP_Arming : : ArmingRequired AP_Arming : : arming_required ( )
{
return ( AP_Arming : : ArmingRequired ) require . get ( ) ;
2013-11-27 22:18:14 -04:00
}
2017-08-09 20:11:22 -03:00
// Copter and sub share the same RC input limits
2017-08-14 14:53:20 -03:00
// Copter checks that min and max have been configured by default, Sub does not
bool AP_Arming : : rc_checks_copter_sub ( const bool display_failure , const RC_Channel * channels [ 4 ] , const bool check_min_max ) const
2017-08-09 20:11:22 -03:00
{
// set rc-checks to success if RC checks are disabled
if ( ( checks_to_perform ! = ARMING_CHECK_ALL ) & & ! ( checks_to_perform & ARMING_CHECK_RC ) ) {
return true ;
}
bool ret = true ;
const char * channel_names [ ] = { " Roll " , " Pitch " , " Throttle " , " Yaw " } ;
for ( uint8_t i = 0 ; i < ARRAY_SIZE ( channel_names ) ; i + + ) {
const RC_Channel * channel = channels [ i ] ;
const char * channel_name = channel_names [ i ] ;
// check if radio has been calibrated
2017-08-14 14:53:20 -03:00
if ( check_min_max & & ! channel - > min_max_configured ( ) ) {
2017-08-09 20:11:22 -03:00
if ( display_failure ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: RC %s not configured " , channel_name ) ;
}
ret = false ;
}
if ( channel - > get_radio_min ( ) > 1300 ) {
if ( display_failure ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: %s radio min too high " , channel_name ) ;
}
ret = false ;
}
if ( channel - > get_radio_max ( ) < 1700 ) {
if ( display_failure ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: %s radio max too low " , channel_name ) ;
}
ret = false ;
}
2017-08-09 20:14:31 -03:00
bool fail = true ;
2017-08-09 20:11:22 -03:00
if ( i = = 2 ) {
// skip checking trim for throttle as older code did not check it
2017-08-09 20:14:31 -03:00
fail = false ;
2017-08-09 20:11:22 -03:00
}
if ( channel - > get_radio_trim ( ) < channel - > get_radio_min ( ) ) {
if ( display_failure ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: %s radio trim below min " , channel_name ) ;
}
2017-08-09 20:14:31 -03:00
if ( fail ) {
ret = false ;
}
2017-08-09 20:11:22 -03:00
}
if ( channel - > get_radio_trim ( ) > channel - > get_radio_max ( ) ) {
if ( display_failure ) {
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " PreArm: %s radio trim above max " , channel_name ) ;
}
2017-08-09 20:14:31 -03:00
if ( fail ) {
ret = false ;
}
2017-08-09 20:11:22 -03:00
}
}
return ret ;
}