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

PX4: support getting servorail voltage from ORB

This commit is contained in:
Andrew Tridgell 2013-09-12 16:28:06 +10:00
parent 8010d7fd78
commit 3a3cecf5ff
2 changed files with 31 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AP_HAL_PX4/AnalogIn.cpp
View File

@ -13,6 +13,8 @@
#include <nuttx/analog/adc.h>
#include <nuttx/config.h>
#include <arch/board/board.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/servorail_status.h>
#define ANLOGIN_DEBUGGING 0
@ -163,7 +165,8 @@ void PX4AnalogIn::init(void* machtnichts)
if (_adc_fd == -1) {
hal.scheduler->panic("Unable to open " ADC_DEVICE_PATH);
}
_battery_handle = orb_subscribe(ORB_ID(battery_status));
_battery_handle = orb_subscribe(ORB_ID(battery_status));
_servorail_handle = orb_subscribe(ORB_ID(servorail_status));
}
/*
@ -208,10 +211,10 @@ void PX4AnalogIn::_timer_tick(void)
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_BATTERY_VOLTAGE_PIN) {
if (c->_pin == PX4_ANALOG_ORB_BATTERY_VOLTAGE_PIN) {
c->_add_value(battery.voltage_v / PX4_VOLTAGE_SCALING);
}
if (c->_pin == PX4_ANALOG_BATTERY_CURRENT_PIN) {
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);
@ -221,6 +224,27 @@ void PX4AnalogIn::_timer_tick(void)
}
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
// 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);
}
if (c->_pin == PX4_ANALOG_ORB_SERVO_VRSSI_PIN) {
c->_add_value(servorail.rssi_v / PX4_VOLTAGE_SCALING);
}
}
}
}
#endif
}
AP_HAL::AnalogSource* PX4AnalogIn::channel(int16_t pin)

5
libraries/AP_HAL_PX4/AnalogIn.h
View File

@ -6,7 +6,6 @@
#include <AP_HAL_PX4.h>
#include <pthread.h>
#include <uORB/uORB.h>
#include <uORB/topics/battery_status.h>
#define PX4_ANALOG_MAX_CHANNELS 16
@ -17,6 +16,8 @@
#define PX4_ANALOG_ORB_BATTERY_CURRENT_PIN 101
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#define PX4_ANALOG_VCC_5V_PIN 4
#define PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN 102
#define PX4_ANALOG_ORB_SERVO_VRSSI_PIN 103
#endif
class PX4::PX4AnalogSource : public AP_HAL::AnalogSource {
@ -57,7 +58,9 @@ public:
private:
int _adc_fd;
int _battery_handle;
int _servorail_handle;
uint64_t _battery_timestamp;
uint64_t _servorail_timestamp;
PX4::PX4AnalogSource* _channels[PX4_ANALOG_MAX_CHANNELS];
uint32_t _last_run;
};