zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Arming: Add rangefinder checks

master
Michael du Breuil 6 years ago committed by Andrew Tridgell
parent
3202a2e0a6
commit
4396540a22
2 changed files with 53 additions and 26 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 40
      libraries/AP_Arming/AP_Arming.cpp
  2. 39
      libraries/AP_Arming/AP_Arming.h

40
libraries/AP_Arming/AP_Arming.cpp
Unescape View File

@ -28,6 +28,7 @@ @@ -28,6 +28,7 @@
#include <AP_InternalError/AP_InternalError.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#if HAL_WITH_UAVCAN
#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
@ -68,15 +69,7 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = { @@ -68,15 +69,7 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = {
AP_PARAM_NO_SHIFT,
AP_PARAM_FRAME_PLANE | AP_PARAM_FRAME_ROVER),
// @Param: CHECK
// @DisplayName: Arm Checks to Peform (bitmask)
// @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.
// @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System
// @Values{Plane}: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,512:Airspeed,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System
// @Bitmask: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System
// @Bitmask{Plane}: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,9:Airspeed,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System
// @User: Standard
AP_GROUPINFO("CHECK", 2, AP_Arming, checks_to_perform, ARMING_CHECK_ALL),
// 2 was the CHECK paramter stored in a AP_Int16
// @Param: ACCTHRESH
// @DisplayName: Accelerometer error threshold
@ -107,6 +100,16 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = { @@ -107,6 +100,16 @@ const AP_Param::GroupInfo AP_Arming::var_info[] = {
// @User: Advanced
AP_GROUPINFO("MIS_ITEMS", 7, AP_Arming, _required_mission_items, 0),
// @Param: CHECK
// @DisplayName: Arm Checks to Peform (bitmask)
// @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.
// @Values: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System
// @Values{Plane}: 0:None,1:All,2:Barometer,4:Compass,8:GPS Lock,16:INS(INertial Sensors - accels & gyros),32:Parameters(unused),64:RC Channels,128:Board voltage,256:Battery Level,512:Airspeed,1024:LoggingAvailable,2048:Hardware safety switch,4096:GPS configuration,8192:System
// @Bitmask: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder
// @Bitmask{Plane}: 0:All,1:Barometer,2:Compass,3:GPS lock,4:INS,5:Parameters,6:RC Channels,7:Board voltage,8:Battery Level,9:Airspeed,10:Logging Available,11:Hardware safety switch,12:GPS Configuration,13:System,14:Mission,15:Rangefinder
// @User: Standard
AP_GROUPINFO("CHECK", 8, AP_Arming, checks_to_perform, ARMING_CHECK_ALL),
AP_GROUPEND
};
@ -594,6 +597,24 @@ bool AP_Arming::mission_checks(bool report) @@ -594,6 +597,24 @@ bool AP_Arming::mission_checks(bool report)
return true;
}
bool AP_Arming::rangefinder_checks(bool report)
{
if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RANGEFINDER)) {
RangeFinder *range = RangeFinder::get_singleton();
if (range == nullptr) {
return true;
}
char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
if (!range->prearm_healthy(buffer, ARRAY_SIZE(buffer))) {
check_failed(ARMING_CHECK_RANGEFINDER, report, "%s", buffer);
return false;
}
}
return true;
}
bool AP_Arming::servo_checks(bool report) const
{
bool check_passed = true;
@ -773,6 +794,7 @@ bool AP_Arming::pre_arm_checks(bool report) @@ -773,6 +794,7 @@ bool AP_Arming::pre_arm_checks(bool report)
& logging_checks(report)
& manual_transmitter_checks(report)
& mission_checks(report)
& rangefinder_checks(report)
& servo_checks(report)
& board_voltage_checks(report)
& system_checks(report)

39
libraries/AP_Arming/AP_Arming.h
Unescape View File

@ -17,22 +17,23 @@ public: @@ -17,22 +17,23 @@ public:
static AP_Arming *get_singleton();
enum ArmingChecks {
ARMING_CHECK_NONE = 0x0000,
ARMING_CHECK_ALL = 0x0001,
ARMING_CHECK_BARO = 0x0002,
ARMING_CHECK_COMPASS = 0x0004,
ARMING_CHECK_GPS = 0x0008,
ARMING_CHECK_INS = 0x0010,
ARMING_CHECK_PARAMETERS = 0x0020,
ARMING_CHECK_RC = 0x0040,
ARMING_CHECK_VOLTAGE = 0x0080,
ARMING_CHECK_BATTERY = 0x0100,
ARMING_CHECK_AIRSPEED = 0x0200,
ARMING_CHECK_LOGGING = 0x0400,
ARMING_CHECK_SWITCH = 0x0800,
ARMING_CHECK_GPS_CONFIG = 0x1000,
ARMING_CHECK_SYSTEM = 0x2000,
ARMING_CHECK_MISSION = 0x4000,
ARMING_CHECK_NONE = 0x0000,
ARMING_CHECK_ALL = (1U << 0),
ARMING_CHECK_BARO = (1U << 1),
ARMING_CHECK_COMPASS = (1U << 2),
ARMING_CHECK_GPS = (1U << 3),
ARMING_CHECK_INS = (1U << 4),
ARMING_CHECK_PARAMETERS = (1U << 5),
ARMING_CHECK_RC = (1U << 6),
ARMING_CHECK_VOLTAGE = (1U << 7),
ARMING_CHECK_BATTERY = (1U << 8),
ARMING_CHECK_AIRSPEED = (1U << 9),
ARMING_CHECK_LOGGING = (1U << 10),
ARMING_CHECK_SWITCH = (1U << 11),
ARMING_CHECK_GPS_CONFIG = (1U << 12),
ARMING_CHECK_SYSTEM = (1U << 13),
ARMING_CHECK_MISSION = (1U << 14),
ARMING_CHECK_RANGEFINDER = (1U << 15),
};
enum class Method {
@ -49,6 +50,8 @@ public: @@ -49,6 +50,8 @@ public:
YES_ZERO_PWM = 2
};
void init(void);
// these functions should not be used by Copter which holds the armed state in the motors library
Required arming_required();
virtual bool arm(AP_Arming::Method method, bool do_arming_checks=true);
@ -84,7 +87,7 @@ protected: @@ -84,7 +87,7 @@ protected:
// Parameters
AP_Int8 require;
AP_Int16 checks_to_perform; // bitmask for which checks are required
AP_Int32 checks_to_perform; // bitmask for which checks are required
AP_Float accel_error_threshold;
AP_Int8 _rudder_arming;
AP_Int32 _required_mission_items;
@ -118,6 +121,8 @@ protected: @@ -118,6 +121,8 @@ protected:
bool mission_checks(bool report);
bool rangefinder_checks(bool report);
bool fence_checks(bool report);
virtual bool system_checks(bool report);

Write Preview
Loading…
Cancel
Save