Copter: change to new board_voltage() API
This commit is contained in:
parent
4e2dbb4a4b
commit
0a3476bbf1
@ -772,11 +772,6 @@ static AP_ServoRelayEvents ServoRelayEvents(relay);
|
||||
// a pin for reading the receiver RSSI voltage.
|
||||
static AP_HAL::AnalogSource* rssi_analog_source;
|
||||
|
||||
|
||||
// Input sources for battery voltage, battery current, board vcc
|
||||
static AP_HAL::AnalogSource* board_vcc_analog_source;
|
||||
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
||||
#endif
|
||||
|
@ -286,7 +286,7 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan)
|
||||
{
|
||||
mavlink_msg_hwstatus_send(
|
||||
chan,
|
||||
board_voltage(),
|
||||
hal.analogin->board_voltage()*1000,
|
||||
hal.i2c->lockup_count());
|
||||
}
|
||||
|
||||
|
@ -224,7 +224,7 @@ static void Log_Write_Current()
|
||||
throttle_integrator : throttle_integrator,
|
||||
battery_voltage : (int16_t) (battery.voltage() * 100.0f),
|
||||
current_amps : (int16_t) (battery.current_amps() * 100.0f),
|
||||
board_voltage : board_voltage(),
|
||||
board_voltage : (uint16_t)(hal.analogin->board_voltage()*1000),
|
||||
current_total : battery.current_total_mah()
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
|
@ -309,11 +309,11 @@
|
||||
#endif
|
||||
|
||||
#ifndef BOARD_VOLTAGE_MIN
|
||||
# define BOARD_VOLTAGE_MIN 4300 // min board voltage in milli volts for pre-arm checks
|
||||
# define BOARD_VOLTAGE_MIN 4.3f // min board voltage in 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
|
||||
# define BOARD_VOLTAGE_MAX 5.8f // max board voltage in volts for pre-arm checks
|
||||
#endif
|
||||
|
||||
// GPS failsafe
|
||||
|
@ -324,7 +324,7 @@ static void pre_arm_checks(bool display_failure)
|
||||
#ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
// check board voltage
|
||||
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
|
||||
if(board_voltage() < BOARD_VOLTAGE_MIN || board_voltage() > BOARD_VOLTAGE_MAX) {
|
||||
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"));
|
||||
}
|
||||
|
@ -160,7 +160,6 @@ static void init_ardupilot()
|
||||
#endif
|
||||
|
||||
rssi_analog_source = hal.analogin->channel(g.rssi_pin);
|
||||
board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
barometer.init();
|
||||
@ -591,14 +590,6 @@ static void check_usb_mux(void)
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
* Read Vcc vs 1.1v internal reference
|
||||
*/
|
||||
uint16_t board_voltage(void)
|
||||
{
|
||||
return board_vcc_analog_source->voltage_latest() * 1000;
|
||||
}
|
||||
|
||||
//
|
||||
// print_flight_mode - prints flight mode to serial port.
|
||||
//
|
||||
|
Loading…
Reference in New Issue
Block a user