dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

px4io: added monitoring of vservo and vrssi 

publish via servorail_status ORB topic
This commit is contained in:
Andrew Tridgell 2013-09-12 15:49:04 +10:00 committed by Lorenz Meier
parent f12794d30e
commit e9e46f9c9d
2 changed files with 52 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/drivers/px4io/px4io.cpp
View File

@ -80,6 +80,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/servorail_status.h>
#include <uORB/topics/parameter_update.h>
#include <debug.h>
@ -260,6 +261,7 @@ private:
orb_advert_t _to_actuators_effective; ///< effective actuator controls topic
orb_advert_t _to_outputs; ///< mixed servo outputs topic
orb_advert_t _to_battery; ///< battery status / voltage
orb_advert_t _to_servorail; ///< servorail status
orb_advert_t _to_safety; ///< status of safety
actuator_outputs_s _outputs; ///<mixed outputs
@ -414,11 +416,21 @@ private:
*
* Publish IO battery information if necessary.
*
* @param vbatt vbattery register
* @param status ibatter register
* @param vbatt vbatt register
* @param ibatt ibatt register
*/
void io_handle_battery(uint16_t vbatt, uint16_t ibatt);
/**
* Handle a servorail update from IO.
*
* Publish servo rail information if necessary.
*
* @param vservo vservo register
* @param vrssi vrssi register
*/
void io_handle_vservo(uint16_t vbatt, uint16_t ibatt);
};
namespace
@ -453,6 +465,7 @@ PX4IO::PX4IO(device::Device *interface) :
_to_actuators_effective(0),
_to_outputs(0),
_to_battery(0),
_to_servorail(0),
_to_safety(0),
_primary_pwm_device(false),
_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
@ -1206,10 +1219,28 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
}
}
void
PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
{
servorail_status_s servorail_status;
servorail_status.timestamp = hrt_absolute_time();
/* voltage is scaled to mV */
servorail_status.voltage_v = vservo * 0.001f;
servorail_status.rssi_v = vrssi * 0.001f;
/* lazily publish the servorail voltages */
if (_to_servorail > 0) {
orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
} else {
_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
}
}
int
PX4IO::io_get_status()
{
uint16_t regs[4];
uint16_t regs[6];
int ret;
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
@ -1224,6 +1255,10 @@ PX4IO::io_get_status()
io_handle_battery(regs[2], regs[3]);
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
io_handle_vservo(regs[4], regs[5]);
#endif
return ret;
}

30
src/modules/px4iofirmware/registers.c
View File

@ -678,27 +678,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
#ifdef ADC_VSERVO
/* PX4IO_P_STATUS_VSERVO */
{
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit:
*
* XXX pending measurements
*
* slope = xxx
* intercept = xxx
*
* Intercept corrected for best results @ 5.0V.
*/
unsigned counts = adc_measure(ADC_VSERVO);
if (counts != 0xffff) {
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000;
r_page_status[PX4IO_P_STATUS_VSERVO] = corrected;
// use 3:1 scaling on 3.3V ADC input
unsigned mV = counts * 9900 / 4096;
r_page_status[PX4IO_P_STATUS_VSERVO] = mV;
}
}
#endif
#ifdef ADC_RSSI
/* PX4IO_P_STATUS_VRSSI */
{
unsigned counts = adc_measure(ADC_RSSI);
if (counts != 0xffff) {
// use 1:1 scaling on 3.3V ADC input
unsigned mV = counts * 3300 / 4096;
r_page_status[PX4IO_P_STATUS_VRSSI] = mV;
}
}
#endif
/* XXX PX4IO_P_STATUS_VRSSI */
/* XXX PX4IO_P_STATUS_PRSSI */
SELECT_PAGE(r_page_status);