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

AP_HAL: added servorail_voltage and power_status_flags() API on AnalogIn

This commit is contained in:
Andrew Tridgell 2014-02-13 22:08:08 +11:00
parent ce43e674fe
commit bea0a46410
3 changed files with 67 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_HAL/AnalogIn.h
View File

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

81
libraries/AP_HAL_PX4/AnalogIn.cpp
View File

@ -15,6 +15,9 @@
#include <arch/board/board.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/servorail_status.h>
#include <uORB/topics/system_power.h>
#include <GCS_MAVLink.h>
#include <errno.h>
#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<PX4_ANALOG_MAX_CHANNELS; j++) {
PX4::PX4AnalogSource *c = _channels[j];
if (c == NULL) continue;
if (c->_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<PX4_ANALOG_MAX_CHANNELS; j++) {
PX4::PX4AnalogSource *c = _channels[j];
if (c == NULL) continue;
if (c->_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<PX4_ANALOG_MAX_CHANNELS; j++) {
PX4::PX4AnalogSource *c = _channels[j];
if (c == NULL) continue;
if (c->_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<PX4_ANALOG_MAX_CHANNELS; j++) {
PX4::PX4AnalogSource *c = _channels[j];
if (c == NULL) continue;
if (c->_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
}

5
libraries/AP_HAL_PX4/AnalogIn.h
View File

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