Copter: pre-arm check for board voltage < 5.8V

This commit is contained in:
Randy Mackay 2013-05-31 11:31:27 +09:00
parent c6e383be0f
commit a1821c89e7
2 changed files with 6 additions and 2 deletions

View File

@ -386,6 +386,10 @@
# define BOARD_VOLTAGE_MIN 4500 // min board voltage in milli volts for pre-arm checks
#endif
#ifndef BOARD_VOLTAGE_MAX
# define BOARD_VOLTAGE_MAX 5800 // max board voltage in milli volts for pre-arm checks
#endif
// Battery failsafe
#ifndef FS_BATTERY
# define FS_BATTERY DISABLED

View File

@ -266,9 +266,9 @@ static void pre_arm_checks(bool display_failure)
#if CONFIG_HAL_BOARD != HAL_BOARD_PX4
// check board voltage
if(board_voltage() < BOARD_VOLTAGE_MIN) {
if(board_voltage() < BOARD_VOLTAGE_MIN || board_voltage() > BOARD_VOLTAGE_MAX) {
if (display_failure) {
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Low Board Voltage"));
gcs_send_text_P(SEVERITY_HIGH,PSTR("PreArm: Check Board Voltage"));
}
return;
}