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/analog/adc.h>
#include <nuttx/config.h> #include <nuttx/config.h>
#include <arch/board/board.h> #include <arch/board/board.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/servorail_status.h>
#define ANLOGIN_DEBUGGING 0 #define ANLOGIN_DEBUGGING 0
@ -163,7 +165,8 @@ void PX4AnalogIn::init(void* machtnichts)
if (_adc_fd == -1) { if (_adc_fd == -1) {
hal.scheduler->panic("Unable to open " ADC_DEVICE_PATH); 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++) { 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;
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); 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 // scale it back to voltage, knowing that the
// px4io code scales by 90.0/5.0 // px4io code scales by 90.0/5.0
c->_add_value(battery.current_a * (5.0f/90.0f) / PX4_VOLTAGE_SCALING); c->_add_value(battery.current_a * (5.0f/90.0f) / PX4_VOLTAGE_SCALING);
@ -221,6 +224,27 @@ void PX4AnalogIn::_timer_tick(void)
} }
#endif #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) 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 <AP_HAL_PX4.h>
#include <pthread.h> #include <pthread.h>
#include <uORB/uORB.h> #include <uORB/uORB.h>
#include <uORB/topics/battery_status.h>
#define PX4_ANALOG_MAX_CHANNELS 16 #define PX4_ANALOG_MAX_CHANNELS 16
@ -17,6 +16,8 @@
#define PX4_ANALOG_ORB_BATTERY_CURRENT_PIN 101 #define PX4_ANALOG_ORB_BATTERY_CURRENT_PIN 101
#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)
#define PX4_ANALOG_VCC_5V_PIN 4 #define PX4_ANALOG_VCC_5V_PIN 4
#define PX4_ANALOG_ORB_SERVO_VOLTAGE_PIN 102
#define PX4_ANALOG_ORB_SERVO_VRSSI_PIN 103
#endif #endif
class PX4::PX4AnalogSource : public AP_HAL::AnalogSource { class PX4::PX4AnalogSource : public AP_HAL::AnalogSource {
@ -57,7 +58,9 @@ public:
private: private:
int _adc_fd; int _adc_fd;
int _battery_handle; int _battery_handle;
int _servorail_handle;
uint64_t _battery_timestamp; uint64_t _battery_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;
}; };