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

AP_Arming: Add servo voltage check, paramertise the Vcc check

This commit is contained in:
Michael du Breuil 2018-11-06 19:29:30 -07:00 committed by Francisco Ferreira
parent 6a41f0c86a
commit e56f12b68e
1 changed files with 19 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Arming/AP_Arming.cpp
View File

@ -17,11 +17,11 @@
#include <AP_Notify/AP_Notify.h>
#include <SRV_Channel/SRV_Channel.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#define AP_ARMING_COMPASS_MAGFIELD_EXPECTED 530
#define AP_ARMING_COMPASS_MAGFIELD_MIN 185 // 0.35 * 530 milligauss
#define AP_ARMING_COMPASS_MAGFIELD_MAX 875 // 1.65 * 530 milligauss
#define AP_ARMING_BOARD_VOLTAGE_MIN 4.3f
#define AP_ARMING_BOARD_VOLTAGE_MAX 5.8f
#define AP_ARMING_ACCEL_ERROR_THRESHOLD 0.75f
#define AP_ARMING_AHRS_GPS_ERROR_MAX 10 // accept up to 10m difference between AHRS and GPS
@ -525,15 +525,29 @@ bool AP_Arming::servo_checks(bool report) const
bool AP_Arming::board_voltage_checks(bool report)
{
#if HAL_HAVE_BOARD_VOLTAGE
// check board voltage
if ((checks_to_perform & ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) {
if(((hal.analogin->board_voltage() < AP_ARMING_BOARD_VOLTAGE_MIN) || (hal.analogin->board_voltage() > AP_ARMING_BOARD_VOLTAGE_MAX))) {
check_failed(ARMING_CHECK_VOLTAGE, report, "Check board voltage");
#if HAL_HAVE_BOARD_VOLTAGE
const float bus_voltage = hal.analogin->board_voltage();
const float vbus_min = AP_BoardConfig::get_minimum_board_voltage();
if(((bus_voltage < vbus_min) || (bus_voltage > AP_ARMING_BOARD_VOLTAGE_MAX))) {
check_failed(ARMING_CHECK_VOLTAGE, report, "Board (%1.1fv) out of range %1.1f-%1.1fv", bus_voltage, vbus_min, AP_ARMING_BOARD_VOLTAGE_MAX);
return false;
}
#endif // HAL_HAVE_BOARD_VOLTAGE
#if HAL_HAVE_SERVO_VOLTAGE
const float vservo_min = AP_BoardConfig::get_minimum_servo_voltage();
if (is_positive(vservo_min)) {
const float servo_voltage = hal.analogin->servorail_voltage();
if (servo_voltage < vservo_min) {
check_failed(ARMING_CHECK_VOLTAGE, report, "Servo voltage to low (%1.2fv < %1.2fv)", servo_voltage, vservo_min);
return false;
}
}
#endif // HAL_HAVE_SERVO_VOLTAGE
}
#endif
return true;
}