Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_HAL: added servorail_voltage and power_status_flags() API on AnalogIn

Browse Source
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 // board 5V rail voltage in volts
virtual float board_voltage(void) = 0; 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 #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 <arch/board/board.h>
#include <uORB/topics/battery_status.h> #include <uORB/topics/battery_status.h>
#include <uORB/topics/servorail_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 #define ANLOGIN_DEBUGGING 0
@ -181,7 +184,9 @@ void PX4AnalogSource::_add_value(float v, float vcc5V)
PX4AnalogIn::PX4AnalogIn() : PX4AnalogIn::PX4AnalogIn() :
_board_voltage(0) _board_voltage(0),
_servorail_voltage(0),
_power_flags(0)
{} {}
void PX4AnalogIn::init(void* machtnichts) void PX4AnalogIn::init(void* machtnichts)
@ -192,6 +197,7 @@ void PX4AnalogIn::init(void* machtnichts)
} }
_battery_handle = orb_subscribe(ORB_ID(battery_status)); _battery_handle = orb_subscribe(ORB_ID(battery_status));
_servorail_handle = orb_subscribe(ORB_ID(servorail_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 // check for new battery data on FMUv1
if (_battery_handle != -1) { if (_battery_handle != -1) {
struct battery_status_s battery; struct battery_status_s battery;
if (orb_copy(ORB_ID(battery_status), _battery_handle, &battery) == OK && bool updated = false;
battery.timestamp != _battery_timestamp) { if (orb_check(_battery_handle, &updated) == 0 && updated) {
_battery_timestamp = battery.timestamp; orb_copy(ORB_ID(battery_status), _battery_handle, &battery);
for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) { if (battery.timestamp != _battery_timestamp) {
PX4::PX4AnalogSource *c = _channels[j]; _battery_timestamp = battery.timestamp;
if (c == NULL) continue; for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
if (c->_pin == PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN) { PX4::PX4AnalogSource *c = _channels[j];
c->_add_value(battery.voltage_v / PX4_VOLTAGE_SCALING, 0); if (c == NULL) continue;
} if (c->_pin == PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN) {
if (c->_pin == PX4_ANALOG_ORB_BATTERY_CURRENT_PIN) { c->_add_value(battery.voltage_v / PX4_VOLTAGE_SCALING, 0);
// scale it back to voltage, knowing that the }
// px4io code scales by 90.0/5.0 if (c->_pin == PX4_ANALOG_ORB_BATTERY_CURRENT_PIN) {
c->_add_value(battery.current_a * (5.0f/90.0f) / PX4_VOLTAGE_SCALING, 0); // 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 // check for new servorail data on FMUv2
if (_servorail_handle != -1) { if (_servorail_handle != -1) {
struct servorail_status_s servorail; struct servorail_status_s servorail;
if (orb_copy(ORB_ID(servorail_status), _servorail_handle, &servorail) == OK && bool updated = false;
servorail.timestamp != _servorail_timestamp) { if (orb_check(_servorail_handle, &updated) == 0 && updated) {
_servorail_timestamp = servorail.timestamp; orb_copy(ORB_ID(servorail_status), _servorail_handle, &servorail);
for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) { if (servorail.timestamp != _servorail_timestamp) {
PX4::PX4AnalogSource *c = _channels[j]; _servorail_timestamp = servorail.timestamp;
if (c == NULL) continue; _servorail_voltage = servorail.voltage_v;
if (c->_pin == PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN) { for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
c->_add_value(servorail.voltage_v / PX4_VOLTAGE_SCALING, 0); PX4::PX4AnalogSource *c = _channels[j];
} if (c == NULL) continue;
if (c->_pin == PX4_ANALOG_ORB_SERVO_VRSSI_PIN) { if (c->_pin == PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN) {
c->_add_value(servorail.rssi_v / PX4_VOLTAGE_SCALING, 0); 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 #endif
} }

5
libraries/AP_HAL_PX4/AnalogIn.h
View File

@ -57,15 +57,20 @@ public:
AP_HAL::AnalogSource* channel(int16_t pin); AP_HAL::AnalogSource* channel(int16_t pin);
void _timer_tick(void); void _timer_tick(void);
float board_voltage(void) { return _board_voltage; } float board_voltage(void) { return _board_voltage; }
float servorail_voltage(void) { return _servorail_voltage; }
uint16_t power_status_flags(void) { return _power_flags; }
private: private:
int _adc_fd; int _adc_fd;
int _battery_handle; int _battery_handle;
int _servorail_handle; int _servorail_handle;
int _system_power_handle;
uint64_t _battery_timestamp; uint64_t _battery_timestamp;
uint64_t _servorail_timestamp; uint64_t _servorail_timestamp;
PX4::PX4AnalogSource* _channels[PX4_ANALOG_MAX_CHANNELS]; PX4::PX4AnalogSource* _channels[PX4_ANALOG_MAX_CHANNELS];
uint32_t _last_run; uint32_t _last_run;
float _board_voltage; float _board_voltage;
float _servorail_voltage;
uint16_t _power_flags;
}; };
#endif // __AP_HAL_PX4_ANALOGIN_H__ #endif // __AP_HAL_PX4_ANALOGIN_H__