Copter: change to new board_voltage() API

This commit is contained in:
Andrew Tridgell 2014-02-13 17:12:10 +11:00
parent 4e2dbb4a4b
commit 0a3476bbf1
6 changed files with 5 additions and 19 deletions

View File

@ -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

View File

@ -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());
}

View File

@ -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));

View File

@ -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

View File

@ -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"));
}

View File

@ -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.
//