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

Move the PX4IO driver into the drivers folder.

Browse Source
This commit is contained in:
px4dev
2012-10-22 21:39:55 -07:00
parent 3f240a70ba
commit 0afeeb5dd8
6 changed files with 1 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/px4io/Makefile
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (C) 2012 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Interface driver for the PX4IO board.
#
APPNAME = px4io
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/drivers/px4io/px4io.cpp
+608
View File
@@ -0,0 +1,608 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file px4io.cpp
* Driver for the PX4IO board.
*
* PX4IO is connected via serial (or possibly some other interface at a later
* point).
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <arch/board/board.h>
#include <drivers/device/device.h>
#include <drivers/drv_rc_input.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <systemlib/perf_counter.h>
#include <systemlib/hx_stream.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/rc_channels.h>
#include "px4io/protocol.h"
#include "uploader.h"
class PX4IO : public device::CDev
{
public:
PX4IO();
~PX4IO();
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
private:
static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS;
int _serial_fd; ///< serial interface to PX4IO
hx_stream_t _io_stream; ///< HX protocol stream
int _task; ///< worker task
volatile bool _task_should_exit;
int _t_actuators; ///< actuator output topic
actuator_controls_s _controls; ///< actuator outputs
int _t_armed; ///< system armed control topic
actuator_armed_s _armed; ///< system armed state
orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
MixerGroup *_mixers; ///< loaded mixers
bool _primary_pwm_device; ///< true if we are the default PWM output
volatile bool _switch_armed; ///< PX4IO switch armed state
// XXX how should this work?
bool _send_needed; ///< If true, we need to send a packet to IO
/**
* Trampoline to the worker task
*/
static void task_main_trampoline(int argc, char *argv[]);
/**
* worker task
*/
void task_main();
/**
* Handle receiving bytes from PX4IO
*/
void io_recv();
/**
* HX protocol callback trampoline.
*/
static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received);
/**
* Callback invoked when we receive a whole packet from PX4IO
*/
void rx_callback(const uint8_t *buffer, size_t bytes_received);
/**
* Send an update packet to PX4IO
*/
void io_send();
/**
* Mixer control callback; invoked to fetch a control from a specific
* group/index during mixing.
*/
static int control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input);
};
namespace
{
PX4IO *g_dev;
}
PX4IO::PX4IO() :
CDev("px4io", "/dev/px4io"),
_serial_fd(-1),
_io_stream(nullptr),
_task(-1),
_task_should_exit(false),
_t_actuators(-1),
_t_armed(-1),
_t_outputs(-1),
_mixers(nullptr),
_primary_pwm_device(false),
_switch_armed(false),
_send_needed(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
_debug_enabled = true;
}
PX4IO::~PX4IO()
{
if (_task != -1) {
/* tell the task we want it to go away */
_task_should_exit = true;
/* spin waiting for the thread to stop */
unsigned i = 10;
do {
/* wait 50ms - it should wake every 100ms or so worst-case */
usleep(50000);
/* if we have given up, kill it */
if (--i == 0) {
task_delete(_task);
break;
}
} while (_task != -1);
}
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
/* kill the HX stream */
if (_io_stream != nullptr)
hx_stream_free(_io_stream);
g_dev = nullptr;
}
int
PX4IO::init()
{
int ret;
ASSERT(_task == -1);
/* do regular cdev init */
ret = CDev::init();
if (ret != OK)
return ret;
/* 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);
if (ret == OK) {
log("default PWM output device");
_primary_pwm_device = true;
}
/* start the IO interface task */
_task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr);
if (_task < 0) {
debug("task start failed: %d", errno);
return -errno;
}
return OK;
}
void
PX4IO::task_main_trampoline(int argc, char *argv[])
{
g_dev->task_main();
}
void
PX4IO::task_main()
{
log("starting");
/* open the serial port */
_serial_fd = ::open("/dev/ttyS2", O_RDWR);
if (_serial_fd < 0) {
debug("failed to open serial port for IO: %d", errno);
_task = -1;
_exit(errno);
}
/* protocol stream */
_io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
perf_counter_t pc_tx_frames = perf_alloc(PC_COUNT, "PX4IO frames transmitted");
perf_counter_t pc_rx_frames = perf_alloc(PC_COUNT, "PX4IO frames received");
perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors");
hx_stream_set_counters(_io_stream, pc_tx_frames, pc_rx_frames, pc_rx_errors);
/* XXX send a who-are-you request */
/* XXX verify firmware/protocol version */
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
* primary PWM output or not.
*/
_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 */
//int update_rate_in_ms = int(1000 / _update_rate);
orb_set_interval(_t_actuators, 20); /* XXX 50Hz hardcoded for now */
_t_armed = orb_subscribe(ORB_ID(actuator_armed));
orb_set_interval(_t_armed, 200); /* 5Hz update rate */
/* advertise the mixed control outputs */
_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
&_outputs);
/* poll descriptor */
pollfd fds[3];
fds[0].fd = _serial_fd;
fds[0].events = POLLIN;
fds[1].fd = _t_actuators;
fds[1].events = POLLIN;
fds[2].fd = _t_armed;
fds[2].events = POLLIN;
log("ready");
/* loop handling received serial bytes */
while (!_task_should_exit) {
/* sleep waiting for data, but no more than 100ms */
int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 1000);
/* this would be bad... */
if (ret < 0) {
log("poll error %d", errno);
usleep(1000000);
continue;
}
/* if we timed out waiting, we should send an update */
if (ret == 0)
_send_needed = true;
if (ret > 0) {
/* if we have new data from IO, go handle it */
if (fds[0].revents & POLLIN)
io_recv();
/* if we have new data from the ORB, go handle it */
if (fds[1].revents & POLLIN) {
/* get controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
/* mix */
if (_mixers != nullptr) {
/* XXX is this the right count? */
_mixers->mix(&_outputs.output[0], _max_actuators);
/* convert to PWM values */
for (unsigned i = 0; i < _max_actuators; i++)
_outputs.output[i] = 1500 + (600 * _outputs.output[i]);
/* and flag for update */
_send_needed = true;
}
}
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(actuator_armed), _t_armed, &_controls);
_send_needed = true;
}
}
/* send an update to IO if required */
if (_send_needed) {
_send_needed = false;
io_send();
}
}
/* tell the dtor that we are exiting */
_task = -1;
_exit(0);
}
int
PX4IO::control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls->control[control_index];
return 0;
}
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;
/* sanity-check the received frame size */
if (bytes_received != sizeof(px4io_report))
return;
/* XXX handle R/C inputs here ... needs code sharing/library */
/* remember the latched arming switch state */
_switch_armed = rep->armed;
unlock();
}
void
PX4IO::io_send()
{
px4io_command cmd;
cmd.f2i_magic = F2I_MAGIC;
/* set outputs */
for (unsigned i = 0; i < _max_actuators; i++)
cmd.servo_command[i] = _outputs.output[i];
/* publish as we send */
orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &_outputs);
// XXX relays
cmd.arm_ok = _armed.armed;
hx_stream_send(_io_stream, &cmd, sizeof(cmd));
}
int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
int ret = OK;
lock();
/* regular ioctl? */
switch (cmd) {
case PWM_SERVO_ARM:
/* fake an armed transition */
_armed.armed = true;
_send_needed = true;
break;
case PWM_SERVO_DISARM:
/* fake a disarmed transition */
_armed.armed = true;
_send_needed = true;
break;
case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
/* fake an update to the selected servo channel */
if ((arg >= 900) && (arg <= 2100)) {
_outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
_send_needed = true;
} else {
ret = -EINVAL;
}
break;
case PWM_SERVO_GET(0) ... PWM_SERVO_GET(_max_actuators - 1):
/* copy the current output value from the channel */
*(servo_position_t *)arg = _outputs.output[cmd - PWM_SERVO_GET(0)];
break;
case MIXERIOCGETOUTPUTCOUNT:
*(unsigned *)arg = _max_actuators;
break;
case MIXERIOCRESET:
if (_mixers != nullptr) {
delete _mixers;
_mixers = nullptr;
}
break;
case MIXERIOCADDSIMPLE: {
mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
/* build the new mixer from the supplied argument */
SimpleMixer *mixer = new SimpleMixer(control_callback,
(uintptr_t)&_controls, mixinfo);
/* validate the new mixer */
if (mixer->check()) {
delete mixer;
ret = -EINVAL;
} else {
/* if we don't have a group yet, allocate one */
if (_mixers == nullptr)
_mixers = new MixerGroup(control_callback,
(uintptr_t)&_controls);
/* add the new mixer to the group */
_mixers->add_mixer(mixer);
}
}
break;
case MIXERIOCADDMULTIROTOR:
/* XXX not yet supported */
ret = -ENOTTY;
break;
case MIXERIOCLOADFILE: {
MixerGroup *newmixers;
const char *path = (const char *)arg;
/* allocate a new mixer group and load it from the file */
newmixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
if (newmixers->load_from_file(path) != 0) {
delete newmixers;
ret = -EINVAL;
}
/* swap the new mixers in for the old */
if (_mixers != nullptr) {
delete _mixers;
}
_mixers = newmixers;
}
break;
default:
/* not a recognised value */
ret = -ENOTTY;
}
unlock();
return ret;
}
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
int
px4io_main(int argc, char *argv[])
{
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr)
errx(1, "already loaded");
/* create the driver - it will set g_dev */
(void)new PX4IO;
if (g_dev == nullptr)
errx(1, "driver alloc failed");
if (OK != g_dev->init()) {
delete g_dev;
errx(1, "driver init failed");
}
exit(0);
}
/* note, stop not currently implemented */
if (!strcmp(argv[1], "update")) {
PX4IO_Uploader *up;
const char *fn[3];
/* work out what we're uploading... */
if (argc > 2) {
fn[0] = argv[2];
fn[1] = nullptr;
} else {
fn[0] = "/fs/microsd/px4io.bin";
fn[1] = "/etc/px4io.bin";
fn[2] = nullptr;
}
up = new PX4IO_Uploader;
int ret = up->upload(&fn[0]);
delete up;
switch (ret) {
case OK:
break;
case -ENOENT:
errx(1, "PX4IO firmware file not found");
case -EEXIST:
case -EIO:
errx(1, "error updating PX4IO - check that bootloader mode is enabled");
case -EINVAL:
errx(1, "verify failed - retry the update");
case -ETIMEDOUT:
errx(1, "timed out waiting for bootloader - power-cycle and try again");
default:
errx(1, "unexpected error %d", ret);
}
return ret;
}
errx(1, "need a verb, only support 'start' and 'update'");
}
apps/drivers/px4io/uploader.cpp
+387
View File
@@ -0,0 +1,387 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Firmware uploader for PX4IO
*/
#include <nuttx/config.h>
#include <sys/types.h>
#include <stdint.h>
#include <stdbool.h>
#include <assert.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <stdarg.h>
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include "uploader.h"
PX4IO_Uploader::PX4IO_Uploader() :
_io_fd(-1),
_fw_fd(-1)
{
}
PX4IO_Uploader::~PX4IO_Uploader()
{
}
int
PX4IO_Uploader::upload(const char *filenames[])
{
int ret;
_io_fd = open("/dev/ttyS2", O_RDWR);
if (_io_fd < 0) {
log("could not open interface");
return -errno;
}
/* look for the bootloader */
ret = sync();
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
return -EIO;
}
for (unsigned i = 0; filenames[i] != nullptr; i++) {
_fw_fd = open(filenames[i], O_RDONLY);
if (_fw_fd < 0) {
log("failed to open %s", filenames[i]);
continue;
}
log("using firmware from %s", filenames[i]);
break;
}
if (_fw_fd == -1)
return -ENOENT;
/* do the usual program thing - allow for failure */
for (unsigned retries = 0; retries < 1; retries++) {
if (retries > 0) {
log("retrying update...");
ret = sync();
if (ret != OK) {
/* this is immediately fatal */
log("bootloader not responding");
return -EIO;
}
}
ret = erase();
if (ret != OK) {
log("erase failed");
continue;
}
ret = program();
if (ret != OK) {
log("program failed");
continue;
}
ret = verify();
if (ret != OK) {
log("verify failed");
continue;
}
ret = reboot();
if (ret != OK) {
log("reboot failed");
return ret;
}
log("update complete");
ret = OK;
break;
}
close(_fw_fd);
return ret;
}
int
PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
{
struct pollfd fds[1];
fds[0].fd = _io_fd;
fds[0].events = POLLIN;
/* wait 100 ms for a character */
int ret = ::poll(&fds[0], 1, timeout);
if (ret < 1) {
//log("poll timeout %d", ret);
return -ETIMEDOUT;
}
read(_io_fd, &c, 1);
//log("recv 0x%02x", c);
return OK;
}
int
PX4IO_Uploader::recv(uint8_t *p, unsigned count)
{
while (count--) {
int ret = recv(*p++);
if (ret != OK)
return ret;
}
return OK;
}
void
PX4IO_Uploader::drain()
{
uint8_t c;
int ret;
do {
ret = recv(c, 10);
//log("discard 0x%02x", c);
} while(ret == OK);
}
int
PX4IO_Uploader::send(uint8_t c)
{
//log("send 0x%02x", c);
if (write(_io_fd, &c, 1) != 1)
return -errno;
return OK;
}
int
PX4IO_Uploader::send(uint8_t *p, unsigned count)
{
while (count--) {
int ret = send(*p++);
if (ret != OK)
return ret;
}
return OK;
}
int
PX4IO_Uploader::get_sync(unsigned timeout)
{
uint8_t c[2];
int ret;
ret = recv(c[0], timeout);
if (ret != OK)
return ret;
ret = recv(c[1], timeout);
if (ret != OK)
return ret;
if ((c[0] != PROTO_INSYNC) || (c[1] != PROTO_OK)) {
log("bad sync 0x%02x,0x%02x", c[0], c[1]);
return -EIO;
}
return OK;
}
int
PX4IO_Uploader::sync()
{
drain();
/* complete any pending program operation */
for (unsigned i = 0; i < (PROG_MULTI_MAX + 6); i++)
send(0);
send(PROTO_GET_SYNC);
send(PROTO_EOC);
return get_sync();
}
int
PX4IO_Uploader::get_info(int param, uint32_t &val)
{
int ret;
send(PROTO_GET_DEVICE);
send(param);
send(PROTO_EOC);
ret = recv((uint8_t *)&val, sizeof(val));
if (ret != OK)
return ret;
return get_sync();
}
int
PX4IO_Uploader::erase()
{
log("erase...");
send(PROTO_CHIP_ERASE);
send(PROTO_EOC);
return get_sync(10000); /* allow 10s timeout */
}
int
PX4IO_Uploader::program()
{
uint8_t file_buf[PROG_MULTI_MAX];
ssize_t count;
int ret;
log("program...");
lseek(_fw_fd, 0, SEEK_SET);
while (true) {
/* get more bytes to program */
//log(" %d", (int)lseek(_fw_fd, 0, SEEK_CUR));
count = read(_fw_fd, file_buf, sizeof(file_buf));
if (count == 0)
return OK;
if (count < 0)
return -errno;
ASSERT((count % 4) == 0);
send(PROTO_PROG_MULTI);
send(count);
send(&file_buf[0], count);
send(PROTO_EOC);
ret = get_sync(1000);
if (ret != OK)
return ret;
}
}
int
PX4IO_Uploader::verify()
{
uint8_t file_buf[PROG_MULTI_MAX];
ssize_t count;
int ret;
log("verify...");
lseek(_fw_fd, 0, SEEK_SET);
send(PROTO_CHIP_VERIFY);
send(PROTO_EOC);
ret = get_sync();
if (ret != OK)
return ret;
while (true) {
/* get more bytes to verify */
int base = (int)lseek(_fw_fd, 0, SEEK_CUR);
count = read(_fw_fd, file_buf, sizeof(file_buf));
if (count == 0)
break;
if (count < 0)
return -errno;
ASSERT((count % 4) == 0);
send(PROTO_READ_MULTI);
send(count);
send(PROTO_EOC);
for (ssize_t i = 0; i < count; i++) {
uint8_t c;
ret = recv(c);
if (ret != OK) {
log("%d: got %d waiting for bytes", base + i, ret);
return ret;
}
if (c != file_buf[i]) {
log("%d: got 0x%02x expected 0x%02x", base + i, c, file_buf[i]);
return -EINVAL;
}
}
ret = get_sync();
if (ret != OK) {
log("timeout waiting for post-verify sync");
return ret;
}
}
return OK;
}
int
PX4IO_Uploader::reboot()
{
send(PROTO_REBOOT);
send(PROTO_EOC);
return OK;
}
int
PX4IO_Uploader::compare(bool &identical)
{
uint32_t file_vectors[15];
uint32_t fw_vectors[15];
int ret;
lseek(_fw_fd, 0, SEEK_SET);
ret = read(_fw_fd, &file_vectors[0], sizeof(file_vectors));
send(PROTO_CHIP_VERIFY);
send(PROTO_EOC);
ret = get_sync();
if (ret != OK)
return ret;
send(PROTO_READ_MULTI);
send(sizeof(fw_vectors));
send(PROTO_EOC);
ret = recv((uint8_t *)&fw_vectors[0], sizeof(fw_vectors));
if (ret != OK)
return ret;
identical = (memcmp(&file_vectors[0], &fw_vectors[0], sizeof(file_vectors))) ? true : false;
return OK;
}
void
PX4IO_Uploader::log(const char *fmt, ...)
{
va_list ap;
printf("[PX4IO] ");
va_start(ap, fmt);
vprintf(fmt, ap);
va_end(ap);
printf("\n");
fflush(stdout);
}
apps/drivers/px4io/uploader.h
+99
View File
@@ -0,0 +1,99 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Firmware uploader for PX4IO
*/
#ifndef _PX4IO_UPLOADER_H
#define _PX4IO_UPLOADER_H value
#include <stdint.h>
#include <stdbool.h>
class PX4IO_Uploader
{
public:
PX4IO_Uploader();
~PX4IO_Uploader();
int upload(const char *filenames[]);
private:
enum {
PROTO_NOP = 0x00,
PROTO_OK = 0x10,
PROTO_FAILED = 0x11,
PROTO_INSYNC = 0x12,
PROTO_EOC = 0x20,
PROTO_GET_SYNC = 0x21,
PROTO_GET_DEVICE = 0x22,
PROTO_CHIP_ERASE = 0x23,
PROTO_CHIP_VERIFY = 0x24,
PROTO_PROG_MULTI = 0x27,
PROTO_READ_MULTI = 0x28,
PROTO_REBOOT = 0x30,
INFO_BL_REV = 1, /**< bootloader protocol revision */
BL_REV = 2, /**< supported bootloader protocol */
INFO_BOARD_ID = 2, /**< board type */
INFO_BOARD_REV = 3, /**< board revision */
INFO_FLASH_SIZE = 4, /**< max firmware size in bytes */
PROG_MULTI_MAX = 60, /**< protocol max is 255, must be multiple of 4 */
READ_MULTI_MAX = 60, /**< protocol max is 255, something overflows with >= 64 */
};
int _io_fd;
int _fw_fd;
void log(const char *fmt, ...);
int recv(uint8_t &c, unsigned timeout = 1000);
int recv(uint8_t *p, unsigned count);
void drain();
int send(uint8_t c);
int send(uint8_t *p, unsigned count);
int get_sync(unsigned timeout = 1000);
int sync();
int get_info(int param, uint32_t &val);
int erase();
int program();
int verify();
int reboot();
int compare(bool &identical);
};
#endif