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

Beat the px4io driver into compilable shape. Just missing RC input configuration now.

Browse Source
This commit is contained in:
px4dev
2013-01-15 23:01:04 -08:00
parent 646b926ac9
commit 7b367c3eb3
2 changed files with 326 additions and 276 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/px4io/px4io.cpp
+320 -276
View File
@@ -58,6 +58,7 @@
#include <arch/board/board.h>
#include <drivers/device/device.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_gpio.h>
@@ -66,7 +67,6 @@
#include <systemlib/mixer/mixer.h>
#include <systemlib/perf_counter.h>
#include <systemlib/hx_stream.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/scheduling_priorities.h>
@@ -94,27 +94,25 @@ public:
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
/**
* Set the PWM via serial update rate
* @warning this directly affects CPU load
*/
int set_pwm_rate(int hz);
bool dump_one;
private:
// XXX
uint16_t _max_actuators;
uint16_t _max_servos;
uint16_t _max_rc_input;
uint16_t _max_relays;
unsigned _max_actuators;
unsigned _max_rc_input;
unsigned _max_relays;
unsigned _max_transfer;
unsigned _update_rate; ///< serial send rate in Hz
unsigned _update_interval; ///< subscription interval limiting send rate
volatile int _task; ///< worker task
volatile bool _task_should_exit;
volatile bool _connected; ///< true once we have received a valid frame
perf_counter_t _perf_update;
/* cached IO state */
uint16_t _status;
uint16_t _alarms;
/* subscribed topics */
int _t_actuators; ///< actuator output topic
int _t_armed; ///< system armed control topic
@@ -157,15 +155,33 @@ private:
/**
* Fetch status and alarms from IO
*
* Also publishes battery voltage/current.
*/
int io_get_status();
/**
* Fetch RC inputs from IO
* Fetch RC inputs from IO.
*
* @param input_rc Input structure to populate.
* @return OK if data was returned.
*/
int io_get_rc_input(rc_input_values &input_rc);
int io_get_raw_rc_input(rc_input_values &input_rc);
/**
* Fetch and publish raw RC input data.
*/
int io_publish_raw_rc();
/**
* Fetch and publish the mixed control values.
*/
int io_publish_mixed_controls();
/**
* Fetch and publish the PWM servo outputs.
*/
int io_publish_pwm_outputs();
/**
* write register(s)
@@ -199,6 +215,16 @@ private:
*/
int io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values);
/**
* read a register
*
* @param page Register page to read from.
* @param offset Register offset to start reading from.
* @return Register value that was read, or _io_reg_get_error on error.
*/
uint32_t io_reg_get(uint8_t page, uint8_t offset);
static const uint32_t _io_reg_get_error = 0x80000000;
/**
* modify a register
*
@@ -209,6 +235,11 @@ private:
*/
int io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits);
/**
* Send mixer definition text to IO
*/
int mixer_send(const char *buf, unsigned buflen);
};
@@ -220,14 +251,16 @@ PX4IO *g_dev;
}
PX4IO::PX4IO() :
CDev("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
dump_one(false),
I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
_max_actuators(0),
_max_servos(0),
_update_rate(50),
_max_rc_input(0),
_max_relays(0),
_max_transfer(16), /* sensible default */
_update_interval(0),
_task(-1),
_task_should_exit(false),
_connected(false),
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
_t_actuators(-1),
_t_armed(-1),
_t_vstatus(-1),
@@ -237,11 +270,7 @@ PX4IO::PX4IO() :
_to_battery(0),
_mix_buf(nullptr),
_mix_buf_len(0),
_primary_pwm_device(false),
_relays(0),
_switch_armed(false),
_send_needed(false),
_config_needed(true)
_primary_pwm_device(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -280,19 +309,22 @@ PX4IO::init()
return ret;
/* get some parameters */
if ((ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT, &_max_actuators, 1)) ||
(_max_actuators < 1) ||
(ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SERVO_COUNT, &_max_servos, 1)) ||
(_max_servos < 1) ||
(ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT, &_max_relays, 1)) ||
(_max_relays < 1) ||
(ret = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT, &_max_rc_input, 1)) ||
(_max_rc_input < 1)) {
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
_max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
_max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER);
_max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
if ((_max_actuators < 1) || (_max_actuators == _io_reg_get_error) ||
(_max_relays < 1) || (_max_relays == _io_reg_get_error) ||
(_max_relays < 16) || (_max_relays == _io_reg_get_error) ||
(_max_rc_input < 1) || (_max_rc_input == _io_reg_get_error)) {
log("failed getting parameters from PX4IO");
return ret;
}
if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
_max_rc_input = RC_INPUT_MAX_CHANNELS;
/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
@@ -327,17 +359,6 @@ PX4IO::init()
return OK;
}
int
PX4IO::set_pwm_rate(int hz)
{
if (hz > 0 && hz <= 400) {
_update_rate = hz;
return OK;
} else {
return -EINVAL;
}
}
void
PX4IO::task_main_trampoline(int argc, char *argv[])
{
@@ -347,11 +368,9 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
void
PX4IO::task_main()
{
hrt_abstime_t last_poll_time = 0;
unsigned poll_phase = 0;
hrt_abstime last_poll_time = 0;
log("starting");
unsigned update_rate_in_ms;
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
@@ -359,10 +378,7 @@ PX4IO::task_main()
*/
_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1));
/* convert the update rate in hz to milliseconds, rounding down if necessary */
update_rate_in_ms = 1000 / _update_rate;
orb_set_interval(_t_actuators, update_rate_in_ms);
orb_set_interval(_t_actuators, 20); /* default to 50Hz */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
@@ -387,10 +403,19 @@ PX4IO::task_main()
/* loop talking to IO */
while (!_task_should_exit) {
/* sleep waiting for topic updates, but no more than 20ms */
/* XXX should actually be calculated to keep the poller running tidily */
/* adjust update interval */
if (_update_interval != 0) {
if (_update_interval < 5)
_update_interval = 5;
if (_update_interval > 100)
_update_interval = 100;
orb_set_interval(_t_actuators, _update_interval);
_update_interval = 0;
}
/* sleep waiting for topic updates, but no more than 100ms */
unlock();
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20);
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
lock();
/* this would be bad... */
@@ -407,7 +432,7 @@ PX4IO::task_main()
if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
io_set_arming_state();
hrt_abstime_t now = hrt_absolute_time();
hrt_abstime now = hrt_absolute_time();
/*
* If this isn't time for the next tick of the polling state machine,
@@ -417,38 +442,27 @@ PX4IO::task_main()
continue;
/*
* Pull status and alarms from IO
* Pull status and alarms from IO.
*/
io_get_status();
switch (poll_phase) {
case 0:
/* XXX fetch raw RC values */
break;
case 1:
/* XXX fetch servo outputs */
break;
}
#if 0
/* advertise the limited control inputs */
memset(&_controls_effective, 0, sizeof(_controls_effective));
_to_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_1),
&_controls_effective);
/* advertise the mixed control outputs */
memset(&_outputs, 0, sizeof(_outputs));
_to_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&_outputs);
#endif
/*
* Get R/C input from IO.
*/
io_publish_raw_rc();
/*
* Fetch mixed servo controls and PWM outputs from IO.
*
* XXX We could do this at a reduced rate in many/most cases.
*/
io_publish_mixed_controls();
io_publish_pwm_outputs();
}
unlock();
out:
debug("exiting");
/* clean up the alternate device node */
@@ -468,13 +482,13 @@ PX4IO::io_set_control_state()
/* get controls */
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &_controls);
ORB_ID(actuator_controls_1), _t_actuators, &controls);
for (unsigned i = 0; i < _max_actuators; i++)
regs[i] = FLOAT_TO_REG(_controls.control[i]);
regs[i] = FLOAT_TO_REG(controls.control[i]);
/* copy values to registers in IO */
io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators);
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators);
}
int
@@ -505,36 +519,40 @@ PX4IO::io_set_arming_state()
clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE;
}
io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
}
int
PX4IO::io_get_status()
{
struct {
uint16_t status;
uint16_t alarms;
uint16_t vbatt;
} state;
uint16_t regs[4];
int ret;
bool rc_valid = false;
/* get STATUS_FLAGS, STATUS_ALARMS and STATUS_VBATT in that order */
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, state, 3);
/* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, regs, sizeof(regs) / sizeof(regs[0]));
if (ret != OK)
return ret;
_status = regs[0];
_alarms = regs[1];
/* XXX handle status */
/* XXX handle alarms */
/* only publish if battery has a valid minimum voltage */
if (state.vbatt > 3300) {
if (regs[2] > 3300) {
battery_status_s battery_status;
battery_status.timestamp = hrt_absolute_time();
battery_status.voltage_v = state.vbatt / 1000.0f;
/* current and discharge are currently (ha) unknown */
battery_status.current_a = -1.0f;
/* voltage is scaled to mV */
battery_status.voltage_v = regs[2] / 1000.0f;
/* current scaling should be to cA in order to avoid limiting at 65A */
battery_status.current_a = regs[3] / 100.f;
/* this requires integration over time - not currently implemented */
battery_status.discharged_mah = -1.0f;
/* lazily publish the battery voltage */
@@ -542,37 +560,13 @@ PX4IO::io_get_status()
orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
} else {
_to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
/*
* If we have RC input, get it
*/
if (state.status & PX4IO_P_STATUS_FLAGS_RC_OK) {
rc_input_values input_rc;
io_get_rc_input(input_rc);
if (state.status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
input_rc.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
} else if (state.status & RC_INPUT_SOURCE_PX4IO_DSM) {
input_rc.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
} else if (state.status & RC_INPUT_SOURCE_PX4IO_SBUS) {
input_rc.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
} else {
input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
}
if (_to_input_rc > 0) {
orb_publish(ORB_ID(input_rc), _to_input_rc, &input_rc);
} else {
_to_input_rc = orb_advertise(ORB_ID(input_rc), &input_rc);
}
}
return ret;
}
int
PX4IO::io_get_rc_input(rc_input_values &input_rc)
PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
{
uint16_t channel_count;
int ret;
@@ -592,38 +586,165 @@ PX4IO::io_get_rc_input(rc_input_values &input_rc)
*
* XXX Since IO has the input calibration info, we ought to be
* able to get the pre-fixed-up controls directly.
*
* XXX can we do this more cheaply? If we knew we had DMA, it would
* almost certainly be better to just get all the inputs...
*/
ret = io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1);
if (ret)
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, &channel_count, 1);
if (ret != OK)
return ret;
input_rc.channel_count = channel_count;
if (channel_count > 0)
ret = io_get_reg(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, channel_count);
ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count);
return ret;
}
int
PX4IO::io_publish_raw_rc()
{
/* if no RC, just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK))
return OK;
/* fetch values from IO */
rc_input_values rc_val;
rc_val.timestamp = hrt_absolute_time();
int ret = io_get_raw_rc_input(rc_val);
if (ret != OK)
return ret;
/* sort out the source of the values */
if (_status & PX4IO_P_STATUS_FLAGS_RC_PPM) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_PPM;
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_DSM) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SPEKTRUM;
} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;
} else {
rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;
}
/* lazily advertise on first publication */
if (_to_input_rc == 0) {
_to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val);
} else {
orb_publish(ORB_ID(input_rc), _to_input_rc, &rc_val);
}
return OK;
}
int
PX4IO::io_publish_mixed_controls()
{
/* if no FMU comms(!) just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
return OK;
/* if not taking raw PPM from us, must be mixing */
if (_status & PX4IO_P_STATUS_FLAGS_RAW_PPM)
return OK;
/* data we are going to fetch */
actuator_controls_effective_s controls_effective;
controls_effective.timestamp = hrt_absolute_time();
/* get actuator controls from IO */
uint16_t act[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_ACTUATORS, 0, act, _max_actuators);
if (ret != OK)
return ret;
/* convert from register format to float */
for (unsigned i = 0; i < _max_actuators; i++)
controls_effective.control_effective[i] = REG_TO_FLOAT(act[i]);
/* laxily advertise on first publication */
if (_to_actuators_effective == 0) {
_to_actuators_effective =
orb_advertise((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
&controls_effective);
} else {
orb_publish((_primary_pwm_device ?
ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE :
ORB_ID(actuator_controls_effective_1)),
_to_actuators_effective, &controls_effective);
}
return OK;
}
int
PX4IO::io_publish_pwm_outputs()
{
/* if no FMU comms(!) just don't publish */
if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
return OK;
/* data we are going to fetch */
actuator_outputs_s outputs;
outputs.timestamp = hrt_absolute_time();
/* get servo values from IO */
uint16_t ctl[_max_actuators];
int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
if (ret != OK)
return ret;
/* convert from register format to float */
for (unsigned i = 0; i < _max_actuators; i++)
outputs.output[i] = REG_TO_FLOAT(ctl[i]);
outputs.noutputs = _max_actuators;
/* lazily advertise on first publication */
if (_to_outputs == 0) {
_to_outputs = orb_advertise((_primary_pwm_device ?
ORB_ID_VEHICLE_CONTROLS :
ORB_ID(actuator_outputs_1)),
&outputs);
} else {
orb_publish((_primary_pwm_device ?
ORB_ID_VEHICLE_CONTROLS :
ORB_ID(actuator_outputs_1)),
_to_outputs,
&outputs);
}
return OK;
}
int
PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
i2c_msg_s msgv[2];
t8_t hdr[2];
uint8_t hdr[2];
hdr[0] = page;
hdr[1] = offset;
mgsv[0].flags = 0;
msgv[0].flags = 0;
msgv[0].buffer = hdr;
msgv[0].length = sizeof(hdr);
msgv[1].flags = 0;
msgv[1].buffer = const_cast<uint8_t *>(values);
msgv[1].buffer = (uint8_t *)(values);
msgv[1].length = num_values * sizeof(*values);
return transfer(msgv, 2);
}
int
PX4IO::io_reg_set(uint8_t page, uint8_t offset, uint16_t value)
{
return io_reg_set(page, offset, &value, 1);
}
int
PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_values)
{
@@ -633,17 +754,28 @@ PX4IO::io_reg_get(uint8_t page, uint8_t offset, uint16_t *values, unsigned num_v
hdr[0] = page;
hdr[1] = offset;
mgsv[0].flags = 0;
msgv[0].flags = 0;
msgv[0].buffer = hdr;
msgv[0].length = sizeof(hdr);
msgv[1].flags = I2C_M_READ;
msgv[1].buffer = values;
msgv[1].buffer = (uint8_t *)values;
msgv[1].length = num_values * sizeof(*values);
return transfer(msgv, 2);
}
uint32_t
PX4IO::io_reg_get(uint8_t page, uint8_t offset)
{
uint16_t value;
if (io_reg_get(page, offset, &value, 1))
return _io_reg_get_error;
return value;
}
int
PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t setbits)
{
@@ -659,104 +791,6 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t
return io_reg_set(page, offset, &value, 1);
}
void
PX4IO::io_recv()
{
uint8_t buf[32];
int count;
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. If more bytes are
* available, we'll go back to poll() again...
*/
count = ::read(_serial_fd, buf, sizeof(buf));
/* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++)
hx_stream_rx(_io_stream, buf[i]);
}
void
PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received)
{
g_dev->rx_callback((const uint8_t *)buffer, bytes_received);
}
void
PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
{
const px4io_report *rep = (const px4io_report *)buffer;
// lock();
/* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report)) {
debug("got %u expected %u", bytes_received, sizeof(px4io_report));
goto out;
}
if (rep->i2f_magic != I2F_MAGIC) {
debug("bad magic");
goto out;
}
_connected = true;
/* publish raw rc channel values from IO if valid channels are present */
if (rep->channel_count > 0) {
_input_rc.timestamp = hrt_absolute_time();
_input_rc.channel_count = rep->channel_count;
for (int i = 0; i < rep->channel_count; i++) {
_input_rc.values[i] = rep->rc_channel[i];
}
orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
}
/* remember the latched arming switch state */
_switch_armed = rep->armed;
/* publish battery information */
/* only publish if battery has a valid minimum voltage */
if (rep->battery_mv > 3300) {
_battery_status.timestamp = hrt_absolute_time();
_battery_status.voltage_v = rep->battery_mv / 1000.0f;
/* current and discharge are unknown */
_battery_status.current_a = -1.0f;
_battery_status.discharged_mah = -1.0f;
/* announce the battery voltage if needed, just publish else */
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);
}
}
_send_needed = true;
/* if monitoring, dump the received info */
if (dump_one) {
dump_one = false;
printf("IO: %s armed ", rep->armed ? "" : "not");
for (unsigned i = 0; i < rep->channel_count; i++)
printf("%d: %d ", i, rep->rc_channel[i]);
printf("\n");
}
out:
// unlock();
return;
}
#if 0
void
PX4IO::config_send()
@@ -812,12 +846,14 @@ PX4IO::config_send()
if (ret)
debug("config error %d", ret);
}
#endif
int
PX4IO::mixer_send(const char *buf, unsigned buflen)
{
uint8_t frame[HX_STREAM_MAX_FRAME];
uint8_t frame[_max_transfer];
px4io_mixdata *msg = (px4io_mixdata *)&frame[0];
unsigned max_len = _max_transfer - sizeof(px4io_mixdata);
msg->f2i_mixer_magic = F2I_MIXER_MAGIC;
msg->action = F2I_MIXER_ACTION_RESET;
@@ -825,8 +861,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
do {
unsigned count = buflen;
if (count > F2I_MIXER_MAX_TEXT)
count = F2I_MIXER_MAX_TEXT;
if (count > max_len)
count = max_len;
if (count > 0) {
memcpy(&msg->text[0], buf, count);
@@ -834,7 +870,20 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
buflen -= count;
}
int ret = hx_stream_send(_io_stream, msg, sizeof(px4io_mixdata) + count);
/*
* We have to send an even number of bytes. This
* will only happen on the very last transfer of a
* mixer, and we are guaranteed that there will be
* space left to round up as _max_transfer will be
* even.
*/
unsigned total_len = sizeof(px4io_mixdata) + count;
if (total_len % 1) {
msg->text[count] = '\0';
total_len++;
}
int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2);
if (ret) {
log("mixer send error %d", ret);
@@ -845,17 +894,19 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
} while (buflen > 0);
return 0;
/* check for the mixer-OK flag */
if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)
return 0;
/* load must have failed for some reason */
return -EINVAL;
}
#endif
int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
int ret = OK;
lock();
/* regular ioctl? */
switch (cmd) {
case PWM_SERVO_ARM:
@@ -877,50 +928,59 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_servos - 1): {
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): {
unsigned channel = cmd - PWM_SERVO_SET(0);
/* send a direct PWM value */
if ((arg >= 900) && (arg <= 2100)) {
ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
} else {
if ((channel >= _max_actuators) || (arg < 900) || (arg > 2100)) {
ret = -EINVAL;
} else {
/* send a direct PWM value */
ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, channel, arg);
}
break;
}
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_servos - 1): {
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(PWM_OUTPUT_MAX_CHANNELS): {
unsigned channel = cmd - PWM_SERVO_GET(0);
/* fetch a current PWM value */
ret = io_reg_get(PX4IO_PAGE_DIRECT_PWM, channel, (uint16_t *)arg, 1);
if (channel >= _max_actuators) {
ret = -EINVAL;
} else {
/* fetch a current PWM value */
uint32_t value = io_reg_get(PX4IO_PAGE_DIRECT_PWM, channel);
if (value == _io_reg_get_error) {
ret = -EIO;
} else {
*(servo_position_t *)arg = value;
}
}
break;
}
case GPIO_RESET: {
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg);
case GPIO_RESET:
ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
break;
}
case GPIO_SET:
arg &= ((1 << _max_relays) - 1);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, value);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
break;
case GPIO_CLEAR:
arg &= ((1 << _max_relays) - 1);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, value, 0);
ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
break;
case GPIO_GET:
ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, (uint16_t *)arg, 1);
*(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS);
if (*(uint32_t *)arg == _io_reg_get_error)
ret = -EIO;
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_servos;
*(unsigned *)arg = _max_actuators;
break;
case MIXERIOCRESET:
@@ -928,23 +988,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
break;
case MIXERIOCLOADBUF:
XXX
/* set the buffer up for transfer */
_mix_buf = (const char *)arg;
_mix_buf_len = strnlen(_mix_buf, 1024);
/* drop the lock and wait for the thread to clear the transmit */
unlock();
while (_mix_buf != nullptr)
usleep(1000);
lock();
ret = 0;
ret = mixer_send((const char *)arg, strnlen(_mix_buf, 1024));
break;
default:
@@ -952,21 +996,19 @@ XXX
ret = -ENOTTY;
}
unlock();
return ret;
}
ssize_t
write(file *filp, const char *buffer, size_t len)
PX4IO::write(file *filp, const char *buffer, size_t len)
{
unsigned count = len / 2;
int ret;
if (count > 0) {
if (count > _max_servos)
count = _max_servos;
ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, buffer, count);
if (count > _max_actuators)
count = _max_actuators;
ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
} else {
ret = -EINVAL;
}
@@ -1034,8 +1076,10 @@ monitor(void)
exit(0);
}
if (g_dev != nullptr)
g_dev->dump_one = true;
#warning implement this
// if (g_dev != nullptr)
// g_dev->dump_one = true;
}
}
@@ -1063,7 +1107,7 @@ px4io_main(int argc, char *argv[])
/* look for the optional pwm update rate for the supported modes */
if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
if (argc > 2 + 1) {
g_dev->set_pwm_rate(atoi(argv[2 + 1]));
#warning implement this
} else {
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
return 1;
apps/px4io/mixer.cpp
+6
View File
@@ -274,6 +274,12 @@ mixer_callback(uintptr_t handle,
return 0;
}
/*
* XXX error handling here should be more aggressive; currently it is
* possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has
* not loaded faithfully.
*/
static char mixer_text[256]; /* large enough for one mixer */
static unsigned mixer_text_length = 0;