fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 15:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'px4/master' into pwm_ioctls

Browse Source 
Conflicts:
	src/drivers/px4io/px4io.cpp
This commit is contained in:
Julian Oes
2013-10-11 14:05:11 +02:00
parent 326f241185 1306c9de7b
commit 2d23d5fd4e
51 changed files with 1097 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/px4io.cpp
+177 -47
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>
@@ -94,6 +95,8 @@ extern device::Device *PX4IO_serial_interface() weak_function;
#define PX4IO_SET_DEBUG _IOC(0xff00, 0)
#define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1)
#define UPDATE_INTERVAL_MIN 2
/**
* The PX4IO class.
*
@@ -177,6 +180,11 @@ public:
*/
int set_failsafe_values(const uint16_t *vals, unsigned len);
/**
* Disable RC input handling
*/
int disable_rc_handling();
/**
* Print IO status.
*
@@ -185,9 +193,9 @@ public:
void print_status();
/**
* Disable RC input handling
* Fetch and print debug console output.
*/
int disable_rc_handling();
int print_debug();
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
/**
@@ -251,6 +259,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
@@ -424,8 +433,27 @@ private:
*/
void dsm_bind_ioctl(int dsmMode);
};
/**
* Handle a battery update from IO.
*
* Publish IO battery information if necessary.
*
* @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
{
@@ -461,6 +489,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
@@ -790,8 +819,8 @@ PX4IO::task_main()
/* adjust update interval */
if (_update_interval != 0) {
if (_update_interval < 5)
_update_interval = 5;
if (_update_interval < UPDATE_INTERVAL_MIN)
_update_interval = UPDATE_INTERVAL_MIN;
if (_update_interval > 100)
_update_interval = 100;
orb_set_interval(_t_actuators, _update_interval);
@@ -1203,10 +1232,67 @@ PX4IO::io_handle_alarms(uint16_t alarms)
return 0;
}
void
PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt)
{
/* only publish if battery has a valid minimum voltage */
if (vbatt <= 3300) {
return;
}
battery_status_s battery_status;
battery_status.timestamp = hrt_absolute_time();
/* voltage is scaled to mV */
battery_status.voltage_v = vbatt / 1000.0f;
/*
ibatt contains the raw ADC count, as 12 bit ADC
value, with full range being 3.3v
*/
battery_status.current_a = ibatt * (3.3f/4096.0f) * _battery_amp_per_volt;
battery_status.current_a += _battery_amp_bias;
/*
integrate battery over time to get total mAh used
*/
if (_battery_last_timestamp != 0) {
_battery_mamphour_total += battery_status.current_a *
(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
}
battery_status.discharged_mah = _battery_mamphour_total;
_battery_last_timestamp = battery_status.timestamp;
/* lazily publish the battery voltage */
if (_to_battery > 0) {
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
}
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 */
@@ -1216,40 +1302,14 @@ PX4IO::io_get_status()
io_handle_status(regs[0]);
io_handle_alarms(regs[1]);
/* only publish if battery has a valid minimum voltage */
if (regs[2] > 3300) {
battery_status_s battery_status;
battery_status.timestamp = hrt_absolute_time();
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
io_handle_battery(regs[2], regs[3]);
#endif
/* voltage is scaled to mV */
battery_status.voltage_v = regs[2] / 1000.0f;
/*
regs[3] contains the raw ADC count, as 12 bit ADC
value, with full range being 3.3v
*/
battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt;
battery_status.current_a += _battery_amp_bias;
/*
integrate battery over time to get total mAh used
*/
if (_battery_last_timestamp != 0) {
_battery_mamphour_total += battery_status.current_a *
(battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600;
}
battery_status.discharged_mah = _battery_mamphour_total;
_battery_last_timestamp = battery_status.timestamp;
/* lazily publish the battery voltage */
if (_to_battery > 0) {
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
}
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
io_handle_vservo(regs[4], regs[5]);
#endif
return ret;
}
@@ -1476,9 +1536,53 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
return io_reg_set(page, offset, value);
}
int
PX4IO::print_debug()
{
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
int io_fd = -1;
if (io_fd < 0) {
io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY);
}
/* read IO's output */
if (io_fd > 0) {
pollfd fds[1];
fds[0].fd = io_fd;
fds[0].events = POLLIN;
usleep(500);
int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0);
if (pret > 0) {
int count;
char buf[65];
do {
count = ::read(io_fd, buf, sizeof(buf) - 1);
if (count > 0) {
/* enforce null termination */
buf[count] = '\0';
warnx("IO CONSOLE: %s", buf);
}
} while (count > 0);
}
::close(io_fd);
return 0;
}
#endif
return 1;
}
int
PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
{
/* get debug level */
int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG);
uint8_t frame[_max_transfer];
@@ -1500,6 +1604,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
memcpy(&msg->text[0], buf, count);
buf += count;
buflen -= count;
} else {
continue;
}
/*
@@ -1517,6 +1623,15 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
/* print mixer chunk */
if (debuglevel > 5 || ret) {
warnx("fmu sent: \"%s\"", msg->text);
/* read IO's output */
print_debug();
}
if (ret) {
log("mixer send error %d", ret);
return ret;
@@ -1526,6 +1641,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
} while (buflen > 0);
/* ensure a closing newline */
msg->text[0] = '\n';
msg->text[1] = '\0';
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2);
retries--;
log("mixer sent");
@@ -1997,8 +2118,8 @@ int
PX4IO::set_update_rate(int rate)
{
int interval_ms = 1000 / rate;
if (interval_ms < 3) {
interval_ms = 3;
if (interval_ms < UPDATE_INTERVAL_MIN) {
interval_ms = UPDATE_INTERVAL_MIN;
warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
}
@@ -2244,28 +2365,37 @@ test(void)
void
monitor(void)
{
/* clear screen */
printf("\033[2J");
unsigned cancels = 3;
printf("Hit <enter> three times to exit monitor mode\n");
for (;;) {
pollfd fds[1];
fds[0].fd = 0;
fds[0].events = POLLIN;
poll(fds, 1, 500);
poll(fds, 1, 2000);
if (fds[0].revents == POLLIN) {
int c;
read(0, &c, 1);
if (cancels-- == 0)
if (cancels-- == 0) {
printf("\033[H"); /* move cursor home and clear screen */
exit(0);
}
}
#warning implement this
if (g_dev != nullptr) {
// if (g_dev != nullptr)
// g_dev->dump_one = true;
printf("\033[H"); /* move cursor home and clear screen */
(void)g_dev->print_status();
(void)g_dev->print_debug();
printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n");
} else {
errx(1, "driver not loaded, exiting");
}
}
}
@@ -2372,7 +2502,7 @@ px4io_main(int argc, char *argv[])
if ((argc > 2)) {
g_dev->set_update_rate(atoi(argv[2]));
} else {
errx(1, "missing argument (50 - 400 Hz)");
errx(1, "missing argument (50 - 500 Hz)");
return 1;
}
exit(0);