Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 02:18:29 -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

41
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,8 +245,10 @@ 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) {
orb_copy(ORB_ID(battery_status), _battery_handle, &battery);
if (battery.timestamp != _battery_timestamp) {
_battery_timestamp = battery.timestamp; _battery_timestamp = battery.timestamp;
for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) { for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
PX4::PX4AnalogSource *c = _channels[j]; PX4::PX4AnalogSource *c = _channels[j];
@ -256,15 +264,19 @@ void PX4AnalogIn::_timer_tick(void)
} }
} }
} }
}
#endif #endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
// 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) {
orb_copy(ORB_ID(servorail_status), _servorail_handle, &servorail);
if (servorail.timestamp != _servorail_timestamp) {
_servorail_timestamp = servorail.timestamp; _servorail_timestamp = servorail.timestamp;
_servorail_voltage = servorail.voltage_v;
for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) { for (uint8_t j=0; j<PX4_ANALOG_MAX_CHANNELS; j++) {
PX4::PX4AnalogSource *c = _channels[j]; PX4::PX4AnalogSource *c = _channels[j];
if (c == NULL) continue; if (c == NULL) continue;
@ -277,6 +289,25 @@ void PX4AnalogIn::_timer_tick(void)
} }
} }
} }
}
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__