From bea0a46410e172a368a2f4668f4336e9adfb2013 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2014 22:08:08 +1100 Subject: [PATCH] AP_HAL: added servorail_voltage and power_status_flags() API on AnalogIn --- libraries/AP_HAL/AnalogIn.h | 6 +++ libraries/AP_HAL_PX4/AnalogIn.cpp | 81 +++++++++++++++++++++---------- libraries/AP_HAL_PX4/AnalogIn.h | 5 ++ 3 files changed, 67 insertions(+), 25 deletions(-) diff --git a/libraries/AP_HAL/AnalogIn.h b/libraries/AP_HAL/AnalogIn.h index e22c27f32e..383d4a2a4f 100644 --- a/libraries/AP_HAL/AnalogIn.h +++ b/libraries/AP_HAL/AnalogIn.h @@ -44,6 +44,12 @@ public: // board 5V rail voltage in volts virtual float board_voltage(void) = 0; + + // servo rail voltage in volts, or 0 if unknown + virtual float servorail_voltage(void) { return 0; } + + // power supply status flags, see MAV_POWER_STATUS + virtual uint16_t power_status_flags(void) { return 0; } }; #define ANALOG_INPUT_BOARD_VCC 254 diff --git a/libraries/AP_HAL_PX4/AnalogIn.cpp b/libraries/AP_HAL_PX4/AnalogIn.cpp index fb82591a07..da9551cd0c 100644 --- a/libraries/AP_HAL_PX4/AnalogIn.cpp +++ b/libraries/AP_HAL_PX4/AnalogIn.cpp @@ -15,6 +15,9 @@ #include #include #include +#include +#include +#include #define ANLOGIN_DEBUGGING 0 @@ -181,7 +184,9 @@ void PX4AnalogSource::_add_value(float v, float vcc5V) PX4AnalogIn::PX4AnalogIn() : - _board_voltage(0) + _board_voltage(0), + _servorail_voltage(0), + _power_flags(0) {} void PX4AnalogIn::init(void* machtnichts) @@ -192,6 +197,7 @@ void PX4AnalogIn::init(void* machtnichts) } _battery_handle = orb_subscribe(ORB_ID(battery_status)); _servorail_handle = orb_subscribe(ORB_ID(servorail_status)); + _system_power_handle = orb_subscribe(ORB_ID(system_power)); } /* @@ -239,19 +245,22 @@ void PX4AnalogIn::_timer_tick(void) // check for new battery data on FMUv1 if (_battery_handle != -1) { struct battery_status_s battery; - if (orb_copy(ORB_ID(battery_status), _battery_handle, &battery) == OK && - battery.timestamp != _battery_timestamp) { - _battery_timestamp = battery.timestamp; - for (uint8_t j=0; j_pin == PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN) { - c->_add_value(battery.voltage_v / PX4_VOLTAGE_SCALING, 0); - } - if (c->_pin == PX4_ANALOG_ORB_BATTERY_CURRENT_PIN) { - // scale it back to voltage, knowing that the - // px4io code scales by 90.0/5.0 - c->_add_value(battery.current_a * (5.0f/90.0f) / PX4_VOLTAGE_SCALING, 0); + bool updated = false; + if (orb_check(_battery_handle, &updated) == 0 && updated) { + orb_copy(ORB_ID(battery_status), _battery_handle, &battery); + if (battery.timestamp != _battery_timestamp) { + _battery_timestamp = battery.timestamp; + for (uint8_t j=0; j_pin == PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN) { + c->_add_value(battery.voltage_v / PX4_VOLTAGE_SCALING, 0); + } + if (c->_pin == PX4_ANALOG_ORB_BATTERY_CURRENT_PIN) { + // scale it back to voltage, knowing that the + // px4io code scales by 90.0/5.0 + c->_add_value(battery.current_a * (5.0f/90.0f) / PX4_VOLTAGE_SCALING, 0); + } } } } @@ -262,21 +271,43 @@ void PX4AnalogIn::_timer_tick(void) // check for new servorail data on FMUv2 if (_servorail_handle != -1) { struct servorail_status_s servorail; - if (orb_copy(ORB_ID(servorail_status), _servorail_handle, &servorail) == OK && - servorail.timestamp != _servorail_timestamp) { - _servorail_timestamp = servorail.timestamp; - for (uint8_t j=0; j_pin == PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN) { - c->_add_value(servorail.voltage_v / PX4_VOLTAGE_SCALING, 0); - } - if (c->_pin == PX4_ANALOG_ORB_SERVO_VRSSI_PIN) { - c->_add_value(servorail.rssi_v / PX4_VOLTAGE_SCALING, 0); + bool updated = false; + if (orb_check(_servorail_handle, &updated) == 0 && updated) { + orb_copy(ORB_ID(servorail_status), _servorail_handle, &servorail); + if (servorail.timestamp != _servorail_timestamp) { + _servorail_timestamp = servorail.timestamp; + _servorail_voltage = servorail.voltage_v; + for (uint8_t j=0; j_pin == PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN) { + c->_add_value(servorail.voltage_v / PX4_VOLTAGE_SCALING, 0); + } + if (c->_pin == PX4_ANALOG_ORB_SERVO_VRSSI_PIN) { + c->_add_value(servorail.rssi_v / PX4_VOLTAGE_SCALING, 0); + } } } } } + if (_system_power_handle != -1) { + struct system_power_s system_power; + bool updated = false; + if (orb_check(_system_power_handle, &updated) == 0 && updated) { + orb_copy(ORB_ID(system_power), _system_power_handle, &system_power); + uint16_t flags = 0; + if (system_power.usb_connected) flags |= MAV_POWER_STATUS_USB_CONNECTED; + if (system_power.brick_valid) flags |= MAV_POWER_STATUS_BRICK_VALID; + if (system_power.servo_valid) flags |= MAV_POWER_STATUS_SERVO_VALID; + if (system_power.periph_5V_OC) flags |= MAV_POWER_STATUS_PERIPH_OVERCURRENT; + if (system_power.hipower_5V_OC) flags |= MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT; + if (_power_flags != 0 && _power_flags != flags) { + // the power status has changed since boot + flags |= MAV_POWER_STATUS_CHANGED; + } + _power_flags = flags; + } + } #endif } diff --git a/libraries/AP_HAL_PX4/AnalogIn.h b/libraries/AP_HAL_PX4/AnalogIn.h index d4f5023f4a..4894c28e23 100644 --- a/libraries/AP_HAL_PX4/AnalogIn.h +++ b/libraries/AP_HAL_PX4/AnalogIn.h @@ -57,15 +57,20 @@ public: AP_HAL::AnalogSource* channel(int16_t pin); void _timer_tick(void); float board_voltage(void) { return _board_voltage; } + float servorail_voltage(void) { return _servorail_voltage; } + uint16_t power_status_flags(void) { return _power_flags; } private: int _adc_fd; int _battery_handle; int _servorail_handle; + int _system_power_handle; uint64_t _battery_timestamp; uint64_t _servorail_timestamp; PX4::PX4AnalogSource* _channels[PX4_ANALOG_MAX_CHANNELS]; uint32_t _last_run; float _board_voltage; + float _servorail_voltage; + uint16_t _power_flags; }; #endif // __AP_HAL_PX4_ANALOGIN_H__