aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
Diffstat (limited to 'apps')
-rw-r--r--apps/commander/commander.c8
-rw-r--r--apps/drivers/boards/px4io/px4io_internal.h4
-rw-r--r--apps/drivers/device/i2c.cpp43
-rw-r--r--apps/drivers/device/i2c.h10
-rw-r--r--apps/drivers/drv_pwm_output.h9
-rw-r--r--apps/drivers/drv_rc_input.h7
-rw-r--r--apps/drivers/hmc5883/hmc5883.cpp2
-rw-r--r--apps/drivers/ms5611/ms5611.cpp61
-rw-r--r--apps/drivers/px4io/px4io.cpp1390
-rw-r--r--apps/px4io/Makefile12
-rw-r--r--apps/px4io/comms.c329
-rw-r--r--apps/px4io/controls.c277
-rw-r--r--apps/px4io/dsm.c44
-rw-r--r--apps/px4io/i2c.c340
-rw-r--r--apps/px4io/mixer.cpp191
-rw-r--r--apps/px4io/protocol.h205
-rw-r--r--apps/px4io/px4io.c130
-rw-r--r--apps/px4io/px4io.h170
-rw-r--r--apps/px4io/registers.c567
-rw-r--r--apps/px4io/safety.c31
-rw-r--r--apps/px4io/sbus.c42
-rw-r--r--apps/systemcmds/i2c/Makefile42
-rw-r--r--apps/systemcmds/i2c/i2c.c140
-rw-r--r--apps/uORB/topics/actuator_controls_effective.h4
-rw-r--r--apps/uORB/topics/vehicle_status.h7
25 files changed, 2866 insertions, 1199 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 5dfb7a945..b18714532 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1260,6 +1260,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
param_t _param_sys_type = param_find("MAV_TYPE");
+ param_t _param_system_id = param_find("MAV_SYS_ID");
+ param_t _param_component_id = param_find("MAV_COMP_ID");
/* welcome user */
warnx("I am in command now!\n");
@@ -1444,6 +1446,7 @@ int commander_thread_main(int argc, char *argv[])
/* parameters changed */
orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
+
/* update parameters */
if (!current_status.flag_system_armed) {
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
@@ -1459,6 +1462,11 @@ int commander_thread_main(int argc, char *argv[])
} else {
current_status.flag_external_manual_override_ok = true;
}
+
+ /* check and update system / component ID */
+ param_get(_param_system_id, &(current_status.system_id));
+ param_get(_param_component_id, &(current_status.component_id));
+
}
}
diff --git a/apps/drivers/boards/px4io/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h
index f77d237a7..44bb98513 100644
--- a/apps/drivers/boards/px4io/px4io_internal.h
+++ b/apps/drivers/boards/px4io/px4io_internal.h
@@ -74,8 +74,8 @@
#define GPIO_ACC_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12)
#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13)
-#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
-#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
+#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
+#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11)
/* Analog inputs ********************************************************************/
diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp
index 474190d83..a416801eb 100644
--- a/apps/drivers/device/i2c.cpp
+++ b/apps/drivers/device/i2c.cpp
@@ -120,7 +120,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
struct i2c_msg_s msgv[2];
unsigned msgs;
int ret;
- unsigned tries = 0;
+ unsigned retry_count = 0;
do {
// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
@@ -154,16 +154,51 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
I2C_SETFREQUENCY(_dev, _frequency);
ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
+ /* success */
if (ret == OK)
break;
- // reset the I2C bus to unwedge on error
- up_i2creset(_dev);
+ /* if we have already retried once, or we are going to give up, then reset the bus */
+ if ((retry_count >= 1) || (retry_count >= _retries))
+ up_i2creset(_dev);
- } while (tries++ < _retries);
+ } while (retry_count++ < _retries);
return ret;
}
+int
+I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
+{
+ int ret;
+ unsigned retry_count = 0;
+
+ /* force the device address into the message vector */
+ for (unsigned i = 0; i < msgs; i++)
+ msgv[i].addr = _address;
+
+
+ do {
+ /*
+ * I2C architecture means there is an unavoidable race here
+ * if there are any devices on the bus with a different frequency
+ * preference. Really, this is pointless.
+ */
+ I2C_SETFREQUENCY(_dev, _frequency);
+ ret = I2C_TRANSFER(_dev, msgv, msgs);
+
+ /* success */
+ if (ret == OK)
+ break;
+
+ /* if we have already retried once, or we are going to give up, then reset the bus */
+ if ((retry_count >= 1) || (retry_count >= _retries))
+ up_i2creset(_dev);
+
+ } while (retry_count++ < _retries);
+
+ return ret;
+}
+
} // namespace device \ No newline at end of file
diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h
index 4d630b8a8..66c34dd7c 100644
--- a/apps/drivers/device/i2c.h
+++ b/apps/drivers/device/i2c.h
@@ -101,6 +101,16 @@ protected:
uint8_t *recv, unsigned recv_len);
/**
+ * Perform a multi-part I2C transaction to the device.
+ *
+ * @param msgv An I2C message vector.
+ * @param msgs The number of entries in the message vector.
+ * @return OK if the transfer was successful, -errno
+ * otherwise.
+ */
+ int transfer(i2c_msg_s *msgv, unsigned msgs);
+
+ /**
* Change the bus address.
*
* Most often useful during probe() when the driver is testing
diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index c110cd5cb..64bb540b4 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -77,9 +77,6 @@ typedef uint16_t servo_position_t;
* device.
*/
struct pwm_output_values {
- /** desired servo update rate in Hz */
- uint32_t update_rate;
-
/** desired pulse widths for each of the supported channels */
servo_position_t values[PWM_OUTPUT_MAX_CHANNELS];
};
@@ -106,6 +103,12 @@ ORB_DECLARE(output_pwm);
/** set update rate in Hz */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
+/** get the number of servos in *(unsigned *)arg */
+#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3)
+
+/** set debug level for servo IO */
+#define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4)
+
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h
index 927406108..4decc324e 100644
--- a/apps/drivers/drv_rc_input.h
+++ b/apps/drivers/drv_rc_input.h
@@ -100,4 +100,11 @@ struct rc_input_values {
*/
ORB_DECLARE(input_rc);
+#define _RC_INPUT_BASE 0x2b00
+
+/** Fetch R/C input values into (rc_input_values *)arg */
+
+#define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0)
+
+
#endif /* _DRV_RC_INPUT_H */
diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 3734d7755..4a201b98c 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -465,7 +465,7 @@ HMC5883::probe()
read_reg(ADDR_ID_C, data[2]))
debug("read_reg fail");
- _retries = 1;
+ _retries = 2;
if ((data[0] != ID_A_WHO_AM_I) ||
(data[1] != ID_B_WHO_AM_I) ||
diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp
index b8845aaf1..30166828a 100644
--- a/apps/drivers/ms5611/ms5611.cpp
+++ b/apps/drivers/ms5611/ms5611.cpp
@@ -162,12 +162,12 @@ private:
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
- void start();
+ void start_cycle();
/**
* Stop the automatic measurement state machine.
*/
- void stop();
+ void stop_cycle();
/**
* Perform a poll cycle; collect from the previous measurement
@@ -287,7 +287,7 @@ MS5611::MS5611(int bus) :
MS5611::~MS5611()
{
/* make sure we are truly inactive */
- stop();
+ stop_cycle();
/* free any existing reports */
if (_reports != nullptr)
@@ -331,7 +331,11 @@ MS5611::probe()
if ((OK == probe_address(MS5611_ADDRESS_1)) ||
(OK == probe_address(MS5611_ADDRESS_2))) {
- _retries = 1;
+ /*
+ * Disable retries; we may enable them selectively in some cases,
+ * but the device gets confused if we retry some of the commands.
+ */
+ _retries = 0;
return OK;
}
@@ -436,7 +440,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
- stop();
+ stop_cycle();
_measure_ticks = 0;
return OK;
@@ -458,7 +462,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
/* if we need to start the poll state machine, do it */
if (want_start)
- start();
+ start_cycle();
return OK;
}
@@ -480,7 +484,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
/* if we need to start the poll state machine, do it */
if (want_start)
- start();
+ start_cycle();
return OK;
}
@@ -508,11 +512,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
- stop();
+ stop_cycle();
delete[] _reports;
_num_reports = arg;
_reports = buf;
- start();
+ start_cycle();
return OK;
}
@@ -545,7 +549,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
}
void
-MS5611::start()
+MS5611::start_cycle()
{
/* reset the report ring and state machine */
@@ -558,7 +562,7 @@ MS5611::start()
}
void
-MS5611::stop()
+MS5611::stop_cycle()
{
work_cancel(HPWORK, &_work);
}
@@ -574,15 +578,25 @@ MS5611::cycle_trampoline(void *arg)
void
MS5611::cycle()
{
+ int ret;
/* collection phase? */
if (_collect_phase) {
/* perform collection */
- if (OK != collect()) {
- log("collection error");
+ ret = collect();
+ if (ret != OK) {
+ if (ret == -6) {
+ /*
+ * The ms5611 seems to regularly fail to respond to
+ * its address; this happens often enough that we'd rather not
+ * spam the console with the message.
+ */
+ } else {
+ log("collection error %d", ret);
+ }
/* reset the collection state machine and try again */
- start();
+ start_cycle();
return;
}
@@ -609,8 +623,13 @@ MS5611::cycle()
}
/* measurement phase */
- if (OK != measure())
- log("measure error");
+ ret = measure();
+ if (ret != OK) {
+ log("measure error %d", ret);
+ /* reset the collection state machine and try again */
+ start_cycle();
+ return;
+ }
/* next phase is collection */
_collect_phase = true;
@@ -635,7 +654,11 @@ MS5611::measure()
/*
* Send the command to begin measuring.
+ *
+ * Disable retries on this command; we can't know whether failure
+ * means the device did or did not see the write.
*/
+ _retries = 0;
ret = transfer(&cmd_data, 1, nullptr, 0);
if (OK != ret)
@@ -647,6 +670,7 @@ MS5611::measure()
int
MS5611::collect()
{
+ int ret;
uint8_t cmd;
uint8_t data[3];
union {
@@ -662,9 +686,10 @@ MS5611::collect()
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time();
- if (OK != transfer(&cmd, 1, &data[0], 3)) {
+ ret = transfer(&cmd, 1, &data[0], 3);
+ if (ret != OK) {
perf_count(_comms_errors);
- return -EIO;
+ return ret;
}
/* fetch the raw value */
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 2c2c236ca..b2e0c6ee4 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2012,2013 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
@@ -35,8 +35,7 @@
* @file px4io.cpp
* Driver for the PX4IO board.
*
- * PX4IO is connected via serial (or possibly some other interface at a later
- * point).
+ * PX4IO is connected via I2C.
*/
#include <nuttx/config.h>
@@ -53,13 +52,13 @@
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
-#include <termios.h>
#include <fcntl.h>
#include <math.h>
#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>
@@ -68,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>
@@ -78,14 +76,16 @@
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/battery_status.h>
+#include <uORB/topics/parameter_update.h>
#include <px4io/protocol.h>
#include "uploader.h"
-class PX4IO : public device::CDev
+class PX4IO : public device::I2C
{
public:
PX4IO();
@@ -94,59 +94,43 @@ public:
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
-
- /**
- * Set the PWM via serial update rate
- * @warning this directly affects CPU load
- */
- int set_pwm_rate(int hz);
-
- bool dump_one;
+ virtual ssize_t write(file *filp, const char *buffer, size_t len);
private:
// XXX
- static const unsigned _max_actuators = PX4IO_CONTROL_CHANNELS;
- unsigned _update_rate; ///< serial send rate in Hz
+ unsigned _max_actuators;
+ unsigned _max_rc_input;
+ unsigned _max_relays;
+ unsigned _max_transfer;
- int _serial_fd; ///< serial interface to PX4IO
- hx_stream_t _io_stream; ///< HX protocol stream
+ 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
- int _t_actuators; ///< actuator output topic
- actuator_controls_s _controls; ///< actuator outputs
+ perf_counter_t _perf_update;
- orb_advert_t _t_actuators_effective; ///< effective actuator controls topic
- actuator_controls_effective_s _controls_effective; ///< effective controls
+ /* cached IO state */
+ uint16_t _status;
+ uint16_t _alarms;
+ /* subscribed topics */
+ int _t_actuators; ///< actuator output topic
int _t_armed; ///< system armed control topic
- actuator_armed_s _armed; ///< system armed state
int _t_vstatus; ///< system / vehicle status
- vehicle_status_s _vstatus; ///< overall system state
+ int _t_param; ///< parameter update topic
+ /* advertised topics */
orb_advert_t _to_input_rc; ///< rc inputs from io
- rc_input_values _input_rc; ///< rc input values
-
+ 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
- battery_status_s _battery_status;///< battery status data
- orb_advert_t _t_outputs; ///< mixed outputs topic
actuator_outputs_s _outputs; ///< mixed outputs
-
- const char *volatile _mix_buf; ///< mixer text buffer
- volatile unsigned _mix_buf_len; ///< size of the mixer text buffer
+ actuator_controls_effective_s _controls_effective; ///< effective controls
bool _primary_pwm_device; ///< true if we are the default PWM output
- uint32_t _relays; ///< state of the PX4IO relays, one bit per relay
-
- 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
- bool _config_needed; ///< if true, we need to set a config update to IO
/**
* Trampoline to the worker task
@@ -159,43 +143,125 @@ private:
void task_main();
/**
- * Handle receiving bytes from PX4IO
+ * Send controls to IO
+ */
+ int io_set_control_state();
+
+ /**
+ * Update IO's arming-related state
*/
- void io_recv();
+ int io_set_arming_state();
/**
- * HX protocol callback trampoline.
+ * Push RC channel configuration to IO.
*/
- static void rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received);
+ int io_set_rc_config();
/**
- * Callback invoked when we receive a whole packet from PX4IO
+ * Fetch status and alarms from IO
+ *
+ * Also publishes battery voltage/current.
*/
- void rx_callback(const uint8_t *buffer, size_t bytes_received);
+ int io_get_status();
/**
- * Send an update packet to PX4IO
+ * Fetch RC inputs from IO.
+ *
+ * @param input_rc Input structure to populate.
+ * @return OK if data was returned.
*/
- void io_send();
+ int io_get_raw_rc_input(rc_input_values &input_rc);
/**
- * Send a config packet to PX4IO
+ * Fetch and publish raw RC input data.
*/
- void config_send();
+ int io_publish_raw_rc();
/**
- * Send a buffer containing mixer text to PX4IO
+ * 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)
+ *
+ * @param page Register page to write to.
+ * @param offset Register offset to start writing at.
+ * @param values Pointer to array of values to write.
+ * @param num_values The number of values to write.
+ * @return Zero if all values were successfully written.
+ */
+ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+
+ /**
+ * write a register
+ *
+ * @param page Register page to write to.
+ * @param offset Register offset to write to.
+ * @param value Value to write.
+ * @return Zero if the value was written successfully.
+ */
+ int io_reg_set(uint8_t page, uint8_t offset, const uint16_t value);
+
+ /**
+ * read register(s)
+ *
+ * @param page Register page to read from.
+ * @param offset Register offset to start reading from.
+ * @param values Pointer to array where values should be stored.
+ * @param num_values The number of values to read.
+ * @return Zero if all values were successfully read.
+ */
+ 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
+ *
+ * @param page Register page to modify.
+ * @param offset Register offset to modify.
+ * @param clearbits Bits to clear in the register.
+ * @param setbits Bits to set in the register.
+ */
+ 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);
/**
- * Mixer control callback; invoked to fetch a control from a specific
- * group/index during mixing.
+ * Handle a status update from IO.
+ *
+ * Publish IO status information if necessary.
+ *
+ * @param status The status register
+ */
+ int io_handle_status(uint16_t status);
+
+ /**
+ * Handle an alarm update from IO.
+ *
+ * Publish IO alarm information if necessary.
+ *
+ * @param alarm The status register
*/
- static int control_callback(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &input);
+ int io_handle_alarms(uint16_t alarms);
+
};
@@ -207,26 +273,25 @@ PX4IO *g_dev;
}
PX4IO::PX4IO() :
- CDev("px4io", "/dev/px4io"),
- dump_one(false),
- _update_rate(50),
- _serial_fd(-1),
- _io_stream(nullptr),
+ I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+ _max_actuators(0),
+ _max_rc_input(0),
+ _max_relays(0),
+ _max_transfer(16), /* sensible default */
+ _update_interval(0),
+ _status(0),
+ _alarms(0),
_task(-1),
_task_should_exit(false),
- _connected(false),
+ _perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
_t_actuators(-1),
- _t_actuators_effective(-1),
_t_armed(-1),
_t_vstatus(-1),
- _t_outputs(-1),
- _mix_buf(nullptr),
- _mix_buf_len(0),
- _primary_pwm_device(false),
- _relays(0),
- _switch_armed(false),
- _send_needed(false),
- _config_needed(true)
+ _to_input_rc(0),
+ _to_actuators_effective(0),
+ _to_outputs(0),
+ _to_battery(0),
+ _primary_pwm_device(false)
{
/* we need this potentially before it could be set in task_main */
g_dev = this;
@@ -260,11 +325,143 @@ PX4IO::init()
ASSERT(_task == -1);
/* do regular cdev init */
- ret = CDev::init();
+ ret = I2C::init();
+ if (ret != OK)
+ return ret;
+ /*
+ * Enable a couple of retries for operations to IO.
+ *
+ * Register read/write operations are intentionally idempotent
+ * so this is safe as designed.
+ */
+ _retries = 2;
+
+ /* get some parameters */
+ _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) - 2;
+ _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
+ if ((_max_actuators < 1) || (_max_actuators > 255) ||
+ (_max_relays < 1) || (_max_relays > 255) ||
+ (_max_transfer < 16) || (_max_transfer > 255) ||
+ (_max_rc_input < 1) || (_max_rc_input > 255)) {
+
+ log("failed getting parameters from PX4IO");
+ return -1;
+ }
+ if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
+ _max_rc_input = RC_INPUT_MAX_CHANNELS;
+
+ /*
+ * Check for IO flight state - if FMU was flagged to be in
+ * armed state, FMU is recovering from an in-air reset.
+ * Read back status and request the commander to arm
+ * in this case.
+ */
+
+ uint16_t reg;
+
+ /* get IO's last seen FMU state */
+ ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, &reg, sizeof(reg));
if (ret != OK)
return ret;
+ if (reg & PX4IO_P_SETUP_ARMING_ARM_OK) {
+
+ /* WARNING: COMMANDER app/vehicle status must be initialized.
+ * If this fails (or the app is not started), worst-case IO
+ * remains untouched (so manual override is still available).
+ */
+
+ int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+ /* fill with initial values, clear updated flag */
+ vehicle_status_s status;
+ uint64_t try_start_time = hrt_absolute_time();
+ bool updated = false;
+
+ /* keep checking for an update, ensure we got a recent state,
+ not something that was published a long time ago. */
+ do {
+ orb_check(vstatus_sub, &updated);
+
+ if (updated) {
+ /* got data, copy and exit loop */
+ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+ break;
+ }
+
+ /* wait 10 ms */
+ usleep(10000);
+
+ /* abort after 5s */
+ if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+ log("failed to recover from in-air restart (1), aborting IO driver init.");
+ return 1;
+ }
+
+ } while (true);
+
+ /* send command to arm system via command API */
+ vehicle_command_s cmd;
+ /* request arming */
+ cmd.param1 = 1.0f;
+ cmd.param2 = 0;
+ cmd.param3 = 0;
+ cmd.param4 = 0;
+ cmd.param5 = 0;
+ cmd.param6 = 0;
+ cmd.param7 = 0;
+ cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM;
+ cmd.target_system = status.system_id;
+ cmd.target_component = status.component_id;
+ cmd.source_system = status.system_id;
+ cmd.source_component = status.component_id;
+ /* ask to confirm command */
+ cmd.confirmation = 1;
+
+ /* send command once */
+ (void)orb_advertise(ORB_ID(vehicle_command), &cmd);
+
+ /* spin here until IO's state has propagated into the system */
+ do {
+ orb_check(vstatus_sub, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status);
+ }
+
+ /* wait 10 ms */
+ usleep(10000);
+
+ /* abort after 5s */
+ if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+ log("failed to recover from in-air restart (2), aborting IO driver init.");
+ return 1;
+ }
+
+ /* keep waiting for state change for 10 s */
+ } while (!status.flag_system_armed);
+
+ /* regular boot, no in-air restart, init IO */
+ } else {
+
+
+ /* dis-arm IO before touching anything */
+ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING,
+ PX4IO_P_SETUP_ARMING_ARM_OK |
+ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
+ PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
+
+ /* publish RC config to IO */
+ ret = io_set_rc_config();
+ if (ret != OK) {
+ log("failed to update RC input config");
+ 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);
@@ -281,36 +478,9 @@ PX4IO::init()
return -errno;
}
- /* wait a second for it to detect IO */
- for (unsigned i = 0; i < 10; i++) {
- if (_connected) {
- debug("PX4IO connected");
- break;
- }
-
- usleep(100000);
- }
-
- if (!_connected) {
- /* error here will result in everything being torn down */
- log("PX4IO not responding");
- return -EIO;
- }
-
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[])
{
@@ -320,39 +490,9 @@ PX4IO::task_main_trampoline(int argc, char *argv[])
void
PX4IO::task_main()
{
- log("starting");
- unsigned update_rate_in_ms;
-
- /* open the serial port */
- _serial_fd = ::open("/dev/ttyS2", O_RDWR);
-
- if (_serial_fd < 0) {
- log("failed to open serial port: %d", errno);
- goto out;
- }
-
- /* 115200bps, no parity, one stop bit */
- {
- struct termios t;
-
- tcgetattr(_serial_fd, &t);
- cfsetspeed(&t, 115200);
- t.c_cflag &= ~(CSTOPB | PARENB);
- tcsetattr(_serial_fd, TCSANOW, &t);
- }
-
- /* protocol stream */
- _io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this);
-
- if (_io_stream == nullptr) {
- log("failed to allocate HX protocol stream");
- goto out;
- }
+ hrt_abstime last_poll_time = 0;
- hx_stream_set_counters(_io_stream,
- perf_alloc(PC_COUNT, "PX4IO frames transmitted"),
- perf_alloc(PC_COUNT, "PX4IO frames received"),
- perf_alloc(PC_COUNT, "PX4IO receive errors"));
+ log("starting");
/*
* Subscribe to the appropriate PWM output topic based on whether we are the
@@ -360,9 +500,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 */
@@ -370,33 +508,18 @@ PX4IO::task_main()
_t_vstatus = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */
- /* advertise the limited control inputs */
- memset(&_controls_effective, 0, sizeof(_controls_effective));
- _t_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));
- _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
- &_outputs);
-
- /* advertise the rc inputs */
- memset(&_input_rc, 0, sizeof(_input_rc));
- _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc);
-
- /* do not advertise the battery status until its clear that a battery is connected */
- memset(&_battery_status, 0, sizeof(_battery_status));
- _to_battery = -1;
+ _t_param = orb_subscribe(ORB_ID(parameter_update));
+ orb_set_interval(_t_param, 500); /* 2Hz update rate max. */
/* poll descriptor */
pollfd fds[4];
- fds[0].fd = _serial_fd;
+ fds[0].fd = _t_actuators;
fds[0].events = POLLIN;
- fds[1].fd = _t_actuators;
+ fds[1].fd = _t_armed;
fds[1].events = POLLIN;
- fds[2].fd = _t_armed;
+ fds[2].fd = _t_vstatus;
fds[2].events = POLLIN;
- fds[3].fd = _t_vstatus;
+ fds[3].fd = _t_param;
fds[3].events = POLLIN;
debug("ready");
@@ -404,12 +527,22 @@ PX4IO::task_main()
/* lock against the ioctl handler */
lock();
- /* loop handling received serial bytes */
+ /* loop talking to IO */
while (!_task_should_exit) {
- /* sleep waiting for data, but no more than 100ms */
+ /* 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 20ms */
unlock();
- int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100);
+ int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 20);
lock();
/* this would be bad... */
@@ -418,76 +551,64 @@ PX4IO::task_main()
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 control data from the ORB, handle it */
- if (fds[1].revents & POLLIN) {
-
- /* get controls */
- orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
- ORB_ID(actuator_controls_1), _t_actuators, &_controls);
-
- /* scale controls to PWM (temporary measure) */
- for (unsigned i = 0; i < _max_actuators; i++)
- _outputs.output[i] = 1500 + (600 * _controls.control[i]);
-
- /* and flag for update */
- _send_needed = true;
- }
-
- /* if we have an arming state update, handle it */
- if (fds[2].revents & POLLIN) {
-
- orb_copy(ORB_ID(actuator_armed), _t_armed, &_armed);
- _send_needed = true;
- }
-
+ perf_begin(_perf_update);
+ hrt_abstime now = hrt_absolute_time();
+
+ /* if we have new control data from the ORB, handle it */
+ if (fds[0].revents & POLLIN)
+ io_set_control_state();
+
+ /* if we have an arming state update, handle it */
+ if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN))
+ io_set_arming_state();
+
+ /*
+ * If it's time for another tick of the polling status machine,
+ * try it now.
+ */
+ if ((now - last_poll_time) >= 20000) {
+
+ /*
+ * Pull status and alarms from IO.
+ */
+ io_get_status();
+
+ /*
+ * Get raw 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();
+
+ /*
+ * If parameters have changed, re-send RC mappings to IO
+ *
+ * XXX this may be a bit spammy
+ */
if (fds[3].revents & POLLIN) {
- orb_copy(ORB_ID(vehicle_status), _t_vstatus, &_vstatus);
- _send_needed = true;
- }
- }
-
- /* send a config packet to IO if required */
- if (_config_needed) {
- _config_needed = false;
- config_send();
- }
+ parameter_update_s pupdate;
- /* send a mixer update if needed */
- if (_mix_buf != nullptr) {
- mixer_send(_mix_buf, _mix_buf_len);
+ /* copy to reset the notification */
+ orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
- /* clear the buffer record so the ioctl handler knows we're done */
- _mix_buf = nullptr;
- _mix_buf_len = 0;
+ /* re-upload RC input config as it may have changed */
+ io_set_rc_config();
+ }
}
- /* send an update to IO if required */
- if (_send_needed) {
- _send_needed = false;
- io_send();
- }
+ perf_end(_perf_update);
}
unlock();
-out:
debug("exiting");
- /* kill the HX stream */
- if (_io_stream != nullptr)
- hx_stream_free(_io_stream);
-
- ::close(_serial_fd);
-
/* clean up the alternate device node */
if (_primary_pwm_device)
unregister_driver(PWM_OUTPUT_DEVICE_PATH);
@@ -498,208 +619,482 @@ out:
}
int
-PX4IO::control_callback(uintptr_t handle,
- uint8_t control_group,
- uint8_t control_index,
- float &input)
+PX4IO::io_set_control_state()
{
- const actuator_controls_s *controls = (actuator_controls_s *)handle;
+ actuator_controls_s controls; ///< actuator outputs
+ uint16_t regs[_max_actuators];
+
+ /* get controls */
+ orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+ ORB_ID(actuator_controls_1), _t_actuators, &controls);
- input = controls->control[control_index];
- return 0;
+ for (unsigned i = 0; i < _max_actuators; i++)
+ regs[i] = FLOAT_TO_REG(controls.control[i]);
+
+ /* copy values to registers in IO */
+ return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators);
}
-void
-PX4IO::io_recv()
+int
+PX4IO::io_set_arming_state()
{
- uint8_t buf[32];
- int count;
+ actuator_armed_s armed; ///< system armed state
+ vehicle_status_s vstatus; ///< overall system state
- /*
- * 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));
+ orb_copy(ORB_ID(actuator_armed), _t_armed, &armed);
+ orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus);
+
+ uint16_t set = 0;
+ uint16_t clear = 0;
+
+ if (armed.armed) {
+ set |= PX4IO_P_SETUP_ARMING_ARM_OK;
+ } else {
+ clear |= PX4IO_P_SETUP_ARMING_ARM_OK;
+ }
+ if (vstatus.flag_vector_flight_mode_ok) {
+ set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
+ } else {
+ clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK;
+ }
+ if (vstatus.flag_external_manual_override_ok) {
+ set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
+ } else {
+ clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
+ }
- /* pass received bytes to the packet decoder */
- for (int i = 0; i < count; i++)
- hx_stream_rx(_io_stream, buf[i]);
+ return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
}
-void
-PX4IO::rx_callback_trampoline(void *arg, const void *buffer, size_t bytes_received)
+int
+PX4IO::io_set_rc_config()
{
- g_dev->rx_callback((const uint8_t *)buffer, bytes_received);
+ unsigned offset = 0;
+ int input_map[_max_rc_input];
+ int32_t ichan;
+ int ret = OK;
+
+ /*
+ * Generate the input channel -> control channel mapping table;
+ * assign RC_MAP_ROLL/PITCH/YAW/THROTTLE to the canonical
+ * controls.
+ */
+ for (unsigned i = 0; i < _max_rc_input; i++)
+ input_map[i] = -1;
+
+ param_get(param_find("RC_MAP_ROLL"), &ichan);
+ if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ input_map[ichan] = 0;
+
+ param_get(param_find("RC_MAP_PITCH"), &ichan);
+ if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ input_map[ichan] = 1;
+
+ param_get(param_find("RC_MAP_YAW"), &ichan);
+ if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ input_map[ichan] = 2;
+
+ param_get(param_find("RC_MAP_THROTTLE"), &ichan);
+ if ((ichan >= 0) && (ichan < (int)_max_rc_input))
+ input_map[ichan] = 3;
+
+ ichan = 4;
+ for (unsigned i = 0; i < _max_rc_input; i++)
+ if (input_map[i] == -1)
+ input_map[i] = ichan++;
+
+ /*
+ * Iterate all possible RC inputs.
+ */
+ for (unsigned i = 0; i < _max_rc_input; i++) {
+ uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
+ char pname[16];
+ float fval;
+
+ /*
+ * RC params are floats, but do only
+ * contain integer values. Do not scale
+ * or cast them, let the auto-typeconversion
+ * do its job here.
+ * Channels: 500 - 2500
+ * Inverted flag: -1 (inverted) or 1 (normal)
+ */
+
+ sprintf(pname, "RC%d_MIN", i + 1);
+ param_get(param_find(pname), &fval);
+ regs[PX4IO_P_RC_CONFIG_MIN] = fval;
+
+ sprintf(pname, "RC%d_TRIM", i + 1);
+ param_get(param_find(pname), &fval);
+ regs[PX4IO_P_RC_CONFIG_CENTER] = fval;
+
+ sprintf(pname, "RC%d_MAX", i + 1);
+ param_get(param_find(pname), &fval);
+ regs[PX4IO_P_RC_CONFIG_MAX] = fval;
+
+ sprintf(pname, "RC%d_DZ", i + 1);
+ param_get(param_find(pname), &fval);
+ regs[PX4IO_P_RC_CONFIG_DEADZONE] = fval;
+
+ regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = input_map[i];
+
+ regs[PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+ sprintf(pname, "RC%d_REV", i + 1);
+ param_get(param_find(pname), &fval);
+
+ /*
+ * This has been taken for the sake of compatibility
+ * with APM's setup / mission planner: normal: 1,
+ * inverted: -1
+ */
+ if (fval < 0) {
+ regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
+ }
+
+ /* send channel config to IO */
+ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
+ if (ret != OK) {
+ log("RC config update failed");
+ break;
+ }
+ offset += PX4IO_P_RC_CONFIG_STRIDE;
+ }
+
+ return ret;
}
-void
-PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
+int
+PX4IO::io_handle_status(uint16_t status)
{
- const px4io_report *rep = (const px4io_report *)buffer;
+ int ret = 1;
+ /**
+ * WARNING: This section handles in-air resets.
+ */
-// lock();
+ /* check for IO reset - force it back to armed if necessary */
+ if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED)
+ && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+ /* set the arming flag */
+ ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC);
- /* sanity-check the received frame size */
- if (bytes_received != sizeof(px4io_report)) {
- debug("got %u expected %u", bytes_received, sizeof(px4io_report));
- goto out;
- }
+ /* set new status */
+ _status = status;
+ _status &= PX4IO_P_STATUS_FLAGS_ARMED;
+ } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
- if (rep->i2f_magic != I2F_MAGIC) {
- debug("bad magic");
- goto out;
+ /* set the sync flag */
+ ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARM_SYNC);
+ /* set new status */
+ _status = status;
+
+ } else {
+ ret = 0;
+
+ /* set new status */
+ _status = status;
}
- _connected = true;
+ return ret;
+}
- /* 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;
+int
+PX4IO::io_handle_alarms(uint16_t alarms)
+{
- for (int i = 0; i < rep->channel_count; i++) {
- _input_rc.values[i] = rep->rc_channel[i];
- }
+ /* XXX handle alarms */
- orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc);
- }
- /* remember the latched arming switch state */
- _switch_armed = rep->armed;
+ /* set new alarms state */
+ _alarms = alarms;
+}
+
+int
+PX4IO::io_get_status()
+{
+ uint16_t regs[4];
+ int ret;
+
+ /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */
+ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &regs[0], sizeof(regs) / sizeof(regs[0]));
+ if (ret != OK)
+ return ret;
- /* publish battery information */
+ io_handle_status(regs[0]);
+ io_handle_alarms(regs[1]);
/* 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 (regs[2] > 3300) {
+ battery_status_s battery_status;
+
+ battery_status.timestamp = hrt_absolute_time();
+
+ /* 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 */
if (_to_battery > 0) {
- orb_publish(ORB_ID(battery_status), _to_battery, &_battery_status);
+ orb_publish(ORB_ID(battery_status), _to_battery, &battery_status);
} else {
- _to_battery = orb_advertise(ORB_ID(battery_status), &_battery_status);
+ _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status);
}
}
+ return ret;
+}
- _send_needed = true;
+int
+PX4IO::io_get_raw_rc_input(rc_input_values &input_rc)
+{
+ uint32_t channel_count;
+ int ret = OK;
- /* if monitoring, dump the received info */
- if (dump_one) {
- dump_one = false;
+ /* we don't have the status bits, so input_source has to be set elsewhere */
+ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN;
+
+ /*
+ * XXX Because the channel count and channel data are fetched
+ * separately, there is a risk of a race between the two
+ * that could leave us with channel data and a count that
+ * are out of sync.
+ * Fixing this would require a guarantee of atomicity from
+ * IO, and a single fetch for both count and channels.
+ *
+ * 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...
+ */
+ channel_count = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
+ if (channel_count == _io_reg_get_error)
+ return -EIO;
+ if (channel_count > RC_INPUT_MAX_CHANNELS)
+ channel_count = RC_INPUT_MAX_CHANNELS;
+ input_rc.channel_count = channel_count;
+
+ if (channel_count > 0) {
+ ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, input_rc.values, channel_count);
+ if (ret == OK)
+ input_rc.timestamp = hrt_absolute_time();
+ }
- printf("IO: %s armed ", rep->armed ? "" : "not");
+ return ret;
+}
- for (unsigned i = 0; i < rep->channel_count; i++)
- printf("%d: %d ", i, rep->rc_channel[i]);
+int
+PX4IO::io_publish_raw_rc()
+{
+ /* if no raw 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;
- printf("\n");
+ /* 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);
}
-out:
-// unlock();
- return;
+ return OK;
}
-void
-PX4IO::io_send()
+int
+PX4IO::io_publish_mixed_controls()
{
- px4io_command cmd;
- int ret;
+ /* if no FMU comms(!) just don't publish */
+ if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
+ return OK;
- cmd.f2i_magic = F2I_MAGIC;
+ /* if not taking raw PPM from us, must be mixing */
+ if (_status & PX4IO_P_STATUS_FLAGS_RAW_PWM)
+ return OK;
+
+ /* data we are going to fetch */
+ actuator_controls_effective_s controls_effective;
+ controls_effective.timestamp = hrt_absolute_time();
- /* set outputs */
- for (unsigned i = 0; i < _max_actuators; i++) {
- cmd.output_control[i] = _outputs.output[i];
+ /* 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);
}
- /* publish as we send */
- _outputs.timestamp = hrt_absolute_time();
- /* XXX needs to be based off post-mix values from the IO side */
- orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &_outputs);
-
- /* update relays */
- for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++)
- cmd.relay_state[i] = (_relays & (1<< i)) ? true : false;
-
- /* armed and not locked down -> arming ok */
- cmd.arm_ok = (_armed.armed && !_armed.lockdown);
- /* indicate that full autonomous position control / vector flight mode is available */
- cmd.vector_flight_mode_ok = _vstatus.flag_vector_flight_mode_ok;
- /* allow manual override on IO (not allowed for multirotors or other systems with SAS) */
- cmd.manual_override_ok = _vstatus.flag_external_manual_override_ok;
- /* set desired PWM output rate */
- cmd.servo_rate = _update_rate;
-
- ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd));
- if (ret)
- debug("send error %d", ret);
+ return OK;
}
-void
-PX4IO::config_send()
+int
+PX4IO::io_publish_pwm_outputs()
{
- px4io_config cfg;
- int ret;
+ /* if no FMU comms(!) just don't publish */
+ if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
+ return OK;
- cfg.f2i_config_magic = F2I_CONFIG_MAGIC;
-
- int val;
-
- /* maintaing the standard order of Roll, Pitch, Yaw, Throttle */
- param_get(param_find("RC_MAP_ROLL"), &val);
- cfg.rc_map[0] = val;
- param_get(param_find("RC_MAP_PITCH"), &val);
- cfg.rc_map[1] = val;
- param_get(param_find("RC_MAP_YAW"), &val);
- cfg.rc_map[2] = val;
- param_get(param_find("RC_MAP_THROTTLE"), &val);
- cfg.rc_map[3] = val;
-
- /* set the individual channel properties */
- char nbuf[16];
- float float_val;
- for (unsigned i = 0; i < 4; i++) {
- sprintf(nbuf, "RC%d_MIN", i + 1);
- param_get(param_find(nbuf), &float_val);
- cfg.rc_min[i] = float_val;
- }
- for (unsigned i = 0; i < 4; i++) {
- sprintf(nbuf, "RC%d_TRIM", i + 1);
- param_get(param_find(nbuf), &float_val);
- cfg.rc_trim[i] = float_val;
- }
- for (unsigned i = 0; i < 4; i++) {
- sprintf(nbuf, "RC%d_MAX", i + 1);
- param_get(param_find(nbuf), &float_val);
- cfg.rc_max[i] = float_val;
- }
- for (unsigned i = 0; i < 4; i++) {
- sprintf(nbuf, "RC%d_REV", i + 1);
- param_get(param_find(nbuf), &float_val);
- cfg.rc_rev[i] = float_val;
+ /* 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);
}
- for (unsigned i = 0; i < 4; i++) {
- sprintf(nbuf, "RC%d_DZ", i + 1);
- param_get(param_find(nbuf), &float_val);
- cfg.rc_dz[i] = float_val;
+
+ return OK;
+}
+
+int
+PX4IO::io_reg_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+{
+ /* range check the transfer */
+ if (num_values > ((_max_transfer) / sizeof(*values))) {
+ debug("io_reg_set: too many registers (%u, max %u)", num_values, _max_transfer / 2);
+ return -EINVAL;
}
- ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg));
+ /* set up the transfer */
+ uint8_t addr[2] = {
+ page,
+ offset
+ };
+ i2c_msg_s msgv[2];
+
+ msgv[0].flags = 0;
+ msgv[0].buffer = addr;
+ msgv[0].length = 2;
+ msgv[1].flags = I2C_M_NORESTART;
+ msgv[1].buffer = (uint8_t *)values;
+ msgv[1].length = num_values * sizeof(*values);
+
+ /* perform the transfer */
+ int ret = transfer(msgv, 2);
+ if (ret != OK)
+ debug("io_reg_set: error %d", ret);
+ return ret;
+}
+
+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)
+{
+ /* set up the transfer */
+ uint8_t addr[2] = {
+ page,
+ offset
+ };
+ i2c_msg_s msgv[2];
+
+ msgv[0].flags = 0;
+ msgv[0].buffer = addr;
+ msgv[0].length = 2;
+ msgv[1].flags = I2C_M_READ;
+ msgv[1].buffer = (uint8_t *)values;
+ msgv[1].length = num_values * sizeof(*values);
+
+ /* perform the transfer */
+ int ret = transfer(msgv, 2);
+ if (ret != OK)
+ debug("io_reg_get: data error %d", ret);
+ return ret;
+}
+
+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)
+{
+ int ret;
+ uint16_t value;
+ ret = io_reg_get(page, offset, &value, 1);
if (ret)
- debug("config error %d", ret);
+ return ret;
+ value &= ~clearbits;
+ value |= setbits;
+
+ return io_reg_set(page, offset, value);
}
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;
@@ -707,8 +1102,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);
@@ -716,7 +1111,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);
@@ -727,7 +1135,16 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
} while (buflen > 0);
- return 0;
+ debug("mixer upload OK");
+
+ /* 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;
+
+ debug("mixer rejected by IO");
+
+ /* load must have failed for some reason */
+ return -EINVAL;
}
int
@@ -735,104 +1152,154 @@ 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;
+ /* set the 'armed' bit */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_ARM_OK);
break;
case PWM_SERVO_DISARM:
- /* fake a disarmed transition */
- _armed.armed = false;
- _send_needed = true;
+ /* clear the 'armed' bit */
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0);
break;
case PWM_SERVO_SET_UPDATE_RATE:
- // not supported yet
- ret = -EINVAL;
+ /* set the requested rate */
+ if ((arg >= 50) && (arg <= 400)) {
+ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE, arg);
+ } else {
+ ret = -EINVAL;
+ }
+ break;
+
+ case PWM_SERVO_GET_COUNT:
+ *(unsigned *)arg = _max_actuators;
break;
- case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
+ case PWM_SERVO_SET_DEBUG:
+ /* set the debug level */
+ ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG, arg);
+ break;
- /* fake an update to the selected 'servo' channel */
- if ((arg >= 900) && (arg <= 2100)) {
- _outputs.output[cmd - PWM_SERVO_SET(0)] = arg;
- _send_needed = true;
+ case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS): {
- } else {
+ unsigned channel = cmd - PWM_SERVO_SET(0);
+ 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(PWM_OUTPUT_MAX_CHANNELS): {
+
+ unsigned channel = cmd - PWM_SERVO_GET(0);
- 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)];
+ if (channel >= _max_actuators) {
+ ret = -EINVAL;
+ } else {
+ /* fetch a current PWM value */
+ uint32_t value = io_reg_get(PX4IO_PAGE_SERVOS, channel);
+ if (value == _io_reg_get_error) {
+ ret = -EIO;
+ } else {
+ *(servo_position_t *)arg = value;
+ }
+ }
break;
+ }
case GPIO_RESET:
- _relays = 0;
- _send_needed = true;
+ 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, arg);
+ break;
+
case GPIO_CLEAR:
- /* make sure only valid bits are being set */
- if ((arg & ((1UL << PX4IO_RELAY_CHANNELS) - 1)) != arg) {
- ret = EINVAL;
- break;
- }
- if (cmd == GPIO_SET) {
- _relays |= arg;
- } else {
- _relays &= ~arg;
- }
- _send_needed = true;
+ arg &= ((1 << _max_relays) - 1);
+ ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
break;
case GPIO_GET:
- *(uint32_t *)arg = _relays;
+ *(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 = PX4IO_CONTROL_CHANNELS;
+ *(unsigned *)arg = _max_actuators;
break;
case MIXERIOCRESET:
ret = 0; /* load always resets */
break;
- case MIXERIOCLOADBUF:
+ case MIXERIOCLOADBUF: {
+ const char *buf = (const char *)arg;
+ ret = mixer_send(buf, strnlen(buf, 1024));
+ break;
+ }
- /* set the buffer up for transfer */
- _mix_buf = (const char *)arg;
- _mix_buf_len = strnlen(_mix_buf, 1024);
+ case RC_INPUT_GET: {
+ uint16_t status;
+ rc_input_values *rc_val = (rc_input_values *)arg;
- /* drop the lock and wait for the thread to clear the transmit */
- unlock();
+ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, &status, 1);
+ if (ret != OK)
+ break;
- while (_mix_buf != nullptr)
- usleep(1000);
+ /* if no R/C input, don't try to fetch anything */
+ if (!(status & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+ ret = -ENOTCONN;
+ break;
+ }
- lock();
+ /* 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;
+ }
- ret = 0;
+ /* read raw R/C input values */
+ ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE, &(rc_val->values[0]), _max_rc_input);
break;
+ }
default:
/* not a recognised value */
ret = -ENOTTY;
}
- unlock();
-
return ret;
}
+ssize_t
+PX4IO::write(file *filp, const char *buffer, size_t len)
+{
+ unsigned count = len / 2;
+
+ if (count > _max_actuators)
+ count = _max_actuators;
+ if (count > 0) {
+ int ret = io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, (uint16_t *)buffer, count);
+ if (ret != OK)
+ return ret;
+ }
+ return count * 2;
+}
+
extern "C" __EXPORT int px4io_main(int argc, char *argv[]);
namespace
@@ -841,35 +1308,58 @@ namespace
void
test(void)
{
- int fd;
+ int fd;
+ unsigned servo_count = 0;
+ unsigned pwm_value = 1000;
+ int direction = 1;
+ int ret;
- fd = open(PWM_OUTPUT_DEVICE_PATH, 0);
+ fd = open("/dev/px4io", O_WRONLY);
- if (fd < 0) {
- puts("open fail");
- exit(1);
- }
+ if (fd < 0)
+ err(1, "failed to open device");
- ioctl(fd, PWM_SERVO_ARM, 0);
- ioctl(fd, PWM_SERVO_SET(0), 1000);
- ioctl(fd, PWM_SERVO_SET(1), 1100);
- ioctl(fd, PWM_SERVO_SET(2), 1200);
- ioctl(fd, PWM_SERVO_SET(3), 1300);
- ioctl(fd, PWM_SERVO_SET(4), 1400);
- ioctl(fd, PWM_SERVO_SET(5), 1500);
- ioctl(fd, PWM_SERVO_SET(6), 1600);
- ioctl(fd, PWM_SERVO_SET(7), 1700);
+ if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count))
+ err(1, "failed to get servo count");
- close(fd);
+ if (ioctl(fd, PWM_SERVO_ARM, 0))
+ err(1, "failed to arm servos");
- actuator_armed_s aa;
+ for (;;) {
- aa.armed = true;
- aa.lockdown = false;
+ /* sweep all servos between 1000..2000 */
+ servo_position_t servos[servo_count];
+ for (unsigned i = 0; i < servo_count; i++)
+ servos[i] = pwm_value;
- orb_advertise(ORB_ID(actuator_armed), &aa);
+ ret = write(fd, servos, sizeof(servos));
+ if (ret != sizeof(servos))
+ err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
- exit(0);
+ if (direction > 0) {
+ if (pwm_value < 2000) {
+ pwm_value++;
+ } else {
+ direction = -1;
+ }
+ } else {
+ if (pwm_value > 1000) {
+ pwm_value--;
+ } else {
+ direction = 1;
+ }
+ }
+
+ /* readback servo values */
+ for (unsigned i = 0; i < servo_count; i++) {
+ servo_position_t value;
+
+ if (ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&value))
+ err(1, "error reading PWM servo %d", i);
+ if (value != servos[i])
+ errx(1, "servo %d readback error, got %u expected %u", i, value, servos[i]);
+ }
+ }
}
void
@@ -893,8 +1383,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;
}
}
@@ -903,6 +1395,10 @@ monitor(void)
int
px4io_main(int argc, char *argv[])
{
+ /* check for sufficient number of arguments */
+ if (argc < 2)
+ goto out;
+
if (!strcmp(argv[1], "start")) {
if (g_dev != nullptr)
@@ -922,7 +1418,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;
@@ -954,6 +1450,27 @@ px4io_main(int argc, char *argv[])
exit(0);
}
+ if (!strcmp(argv[1], "debug")) {
+ if (argc <= 2) {
+ printf("usage: px4io debug LEVEL\n");
+ exit(1);
+ }
+ if (g_dev == nullptr) {
+ printf("px4io is not started\n");
+ exit(1);
+ }
+ uint8_t level = atoi(argv[2]);
+ // we can cheat and call the driver directly, as it
+ // doesn't reference filp in ioctl()
+ int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level);
+ if (ret != 0) {
+ printf("SET_DEBUG failed - %d\n", ret);
+ exit(1);
+ }
+ printf("SET_DEBUG %u OK\n", (unsigned)level);
+ exit(0);
+ }
+
/* note, stop not currently implemented */
if (!strcmp(argv[1], "update")) {
@@ -1019,5 +1536,6 @@ px4io_main(int argc, char *argv[])
if (!strcmp(argv[1], "monitor"))
monitor();
- errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor' or 'update'");
+ out:
+ errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug' or 'update'");
}
diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile
index d97f963ab..0eb3ffcf7 100644
--- a/apps/px4io/Makefile
+++ b/apps/px4io/Makefile
@@ -39,11 +39,19 @@
# Note that we pull a couple of specific files from the systemlib, since
# we can't support it all.
#
-CSRCS = $(wildcard *.c) \
+CSRCS = adc.c \
+ controls.c \
+ dsm.c \
+ i2c.c \
+ px4io.c \
+ registers.c \
+ safety.c \
+ sbus.c \
../systemlib/hx_stream.c \
../systemlib/perf_counter.c \
../systemlib/up_cxxinitialize.c
-CXXSRCS = $(wildcard *.cpp)
+
+CXXSRCS = mixer.cpp
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c
deleted file mode 100644
index e51c2771a..000000000
--- a/apps/px4io/comms.c
+++ /dev/null
@@ -1,329 +0,0 @@
-/****************************************************************************
- *
- * 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 comms.c
- *
- * FMU communication for the PX4IO module.
- */
-
-#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <debug.h>
-#include <stdlib.h>
-#include <errno.h>
-#include <string.h>
-#include <poll.h>
-#include <termios.h>
-
-#include <nuttx/clock.h>
-
-#include <drivers/drv_hrt.h>
-#include <drivers/drv_pwm_output.h>
-#include <systemlib/hx_stream.h>
-#include <systemlib/perf_counter.h>
-
-#define DEBUG
-#include "px4io.h"
-
-#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */
-#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */
-
-#define FMU_STATUS_INTERVAL 1000000 /* 100ms */
-
-static int fmu_fd;
-static hx_stream_t stream;
-static struct px4io_report report;
-
-static void comms_handle_frame(void *arg, const void *buffer, size_t length);
-
-perf_counter_t comms_rx_errors;
-
-static void
-comms_init(void)
-{
- /* initialise the FMU interface */
- fmu_fd = open("/dev/ttyS1", O_RDWR);
- stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
-
- comms_rx_errors = perf_alloc(PC_COUNT, "rx_err");
- hx_stream_set_counters(stream, 0, 0, comms_rx_errors);
-
- /* default state in the report to FMU */
- report.i2f_magic = I2F_MAGIC;
-
- struct termios t;
-
- /* 115200bps, no parity, one stop bit */
- tcgetattr(fmu_fd, &t);
- cfsetspeed(&t, 115200);
- t.c_cflag &= ~(CSTOPB | PARENB);
- tcsetattr(fmu_fd, TCSANOW, &t);
-
- /* init the ADC */
- adc_init();
-}
-
-void
-comms_main(void)
-{
- comms_init();
-
- struct pollfd fds;
- fds.fd = fmu_fd;
- fds.events = POLLIN;
- debug("FMU: ready");
-
- for (;;) {
- /* wait for serial data, but no more than 10ms */
- poll(&fds, 1, 10);
-
- /*
- * Pull bytes from FMU and feed them to the HX engine.
- * Limit the number of bytes we actually process on any one iteration.
- */
- if (fds.revents & POLLIN) {
- char buf[32];
- ssize_t count = read(fmu_fd, buf, sizeof(buf));
-
- for (int i = 0; i < count; i++)
- hx_stream_rx(stream, buf[i]);
- }
-
- /*
- * Decide if it's time to send an update to the FMU.
- */
- static hrt_abstime last_report_time;
- hrt_abstime now, delta;
-
- /* should we send a report to the FMU? */
- now = hrt_absolute_time();
- delta = now - last_report_time;
-
- if ((delta > FMU_MIN_REPORT_INTERVAL) &&
- (system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
-
- system_state.fmu_report_due = false;
- last_report_time = now;
-
- /* populate the report */
- for (unsigned i = 0; i < system_state.rc_channels; i++) {
- report.rc_channel[i] = system_state.rc_channel_data[i];
- }
-
- report.channel_count = system_state.rc_channels;
- report.armed = system_state.armed;
-
- report.battery_mv = system_state.battery_mv;
- report.adc_in = system_state.adc_in5;
- report.overcurrent = system_state.overcurrent;
-
- /* and send it */
- hx_stream_send(stream, &report, sizeof(report));
- }
-
- /*
- * Fetch ADC values, check overcurrent flags, etc.
- */
- static hrt_abstime last_status_time;
-
- if ((now - last_status_time) > FMU_STATUS_INTERVAL) {
-
- /*
- * Coefficients here derived by measurement of the 5-16V
- * range on one unit:
- *
- * V counts
- * 5 1001
- * 6 1219
- * 7 1436
- * 8 1653
- * 9 1870
- * 10 2086
- * 11 2303
- * 12 2522
- * 13 2738
- * 14 2956
- * 15 3172
- * 16 3389
- *
- * slope = 0.0046067
- * intercept = 0.3863
- *
- * Intercept corrected for best results @ 12V.
- */
- unsigned counts = adc_measure(ADC_VBATT);
- system_state.battery_mv = (4150 + (counts * 46)) / 10;
-
- system_state.adc_in5 = adc_measure(ADC_IN5);
-
- system_state.overcurrent =
- (OVERCURRENT_SERVO ? (1 << 0) : 0) |
- (OVERCURRENT_ACC ? (1 << 1) : 0);
-
- last_status_time = now;
- }
- }
-}
-
-static void
-comms_handle_config(const void *buffer, size_t length)
-{
- const struct px4io_config *cfg = (struct px4io_config *)buffer;
-
- if (length != sizeof(*cfg))
- return;
-
- /* fetch the rc mappings */
- for (unsigned i = 0; i < 4; i++) {
- system_state.rc_map[i] = cfg->rc_map[i];
- }
-
- /* fetch the rc channel attributes */
- for (unsigned i = 0; i < 4; i++) {
- system_state.rc_min[i] = cfg->rc_min[i];
- system_state.rc_trim[i] = cfg->rc_trim[i];
- system_state.rc_max[i] = cfg->rc_max[i];
- system_state.rc_rev[i] = cfg->rc_rev[i];
- system_state.rc_dz[i] = cfg->rc_dz[i];
- }
-}
-
-static void
-comms_handle_command(const void *buffer, size_t length)
-{
- const struct px4io_command *cmd = (struct px4io_command *)buffer;
-
- if (length != sizeof(*cmd))
- return;
-
- irqstate_t flags = irqsave();
-
- /* fetch new PWM output values */
- for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
- system_state.fmu_channel_data[i] = cmd->output_control[i];
-
- /* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
- if (system_state.arm_ok && !cmd->arm_ok)
- system_state.armed = false;
-
- system_state.arm_ok = cmd->arm_ok;
- system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
- system_state.manual_override_ok = cmd->manual_override_ok;
- system_state.mixer_fmu_available = true;
- system_state.fmu_data_received_time = hrt_absolute_time();
-
- /* set PWM update rate if changed (after limiting) */
- uint16_t new_servo_rate = cmd->servo_rate;
-
- /* reject faster than 500 Hz updates */
- if (new_servo_rate > 500) {
- new_servo_rate = 500;
- }
-
- /* reject slower than 50 Hz updates */
- if (new_servo_rate < 50) {
- new_servo_rate = 50;
- }
-
- if (system_state.servo_rate != new_servo_rate) {
- up_pwm_servo_set_rate(new_servo_rate);
- system_state.servo_rate = new_servo_rate;
- }
-
- /*
- * update servo values immediately.
- * the updates are done in addition also
- * in the mainloop, since this function will only
- * update with a connected FMU.
- */
- mixer_tick();
-
- /* handle relay state changes here */
- for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
- if (system_state.relays[i] != cmd->relay_state[i]) {
- switch (i) {
- case 0:
- POWER_ACC1(cmd->relay_state[i]);
- break;
-
- case 1:
- POWER_ACC2(cmd->relay_state[i]);
- break;
-
- case 2:
- POWER_RELAY1(cmd->relay_state[i]);
- break;
-
- case 3:
- POWER_RELAY2(cmd->relay_state[i]);
- break;
- }
- }
-
- system_state.relays[i] = cmd->relay_state[i];
- }
-
- irqrestore(flags);
-}
-
-static void
-comms_handle_frame(void *arg, const void *buffer, size_t length)
-{
- const uint16_t *type = (const uint16_t *)buffer;
-
-
- /* make sure it's what we are expecting */
- if (length > 2) {
- switch (*type) {
- case F2I_MAGIC:
- comms_handle_command(buffer, length);
- break;
-
- case F2I_CONFIG_MAGIC:
- comms_handle_config(buffer, length);
- break;
-
- case F2I_MIXER_MAGIC:
- mixer_handle_text(buffer, length);
- break;
-
- default:
- break;
- }
- }
-}
-
diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c
index 169d5bb81..b4a18bae6 100644
--- a/apps/px4io/controls.c
+++ b/apps/px4io/controls.c
@@ -37,34 +37,22 @@
* R/C inputs and servo outputs.
*/
-
#include <nuttx/config.h>
-#include <stdio.h>
#include <stdbool.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <debug.h>
-#include <stdlib.h>
-#include <errno.h>
-#include <termios.h>
-#include <string.h>
#include <poll.h>
-#include <nuttx/clock.h>
-
#include <drivers/drv_hrt.h>
-#include <systemlib/hx_stream.h>
#include <systemlib/perf_counter.h>
#include <systemlib/ppm_decode.h>
-#define DEBUG
+//#define DEBUG
#include "px4io.h"
#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */
-#define RC_CHANNEL_HIGH_THRESH 1700
-#define RC_CHANNEL_LOW_THRESH 1300
+#define RC_CHANNEL_HIGH_THRESH 5000
+#define RC_CHANNEL_LOW_THRESH -5000
-static void ppm_input(void);
+static bool ppm_input(uint16_t *values, uint16_t *num_values);
void
controls_main(void)
@@ -79,9 +67,27 @@ controls_main(void)
fds[1].fd = sbus_init("/dev/ttyS2");
fds[1].events = POLLIN;
+ ASSERT(fds[0].fd >= 0);
+ ASSERT(fds[1].fd >= 0);
+
+ /* default to a 1:1 input map, all enabled */
+ for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+ unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
+
+ r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
+ r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000;
+ r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500;
+ r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000;
+ r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30;
+ r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
+ r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+ }
+
for (;;) {
/* run this loop at ~100Hz */
- poll(fds, 2, 10);
+ int result = poll(fds, 2, 10);
+
+ ASSERT(result >= 0);
/*
* Gather R/C control inputs from supported sources.
@@ -90,95 +96,220 @@ controls_main(void)
* one control input source, they're going to fight each
* other. Don't do that.
*/
- bool locked = false;
+
+ bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
+ if (dsm_updated)
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+
+ bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
+ if (sbus_updated)
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
/*
- * Store RC channel count to detect switch to RC loss sooner
- * than just by timeout
+ * XXX each S.bus frame will cause a PPM decoder interrupt
+ * storm (lots of edges). It might be sensible to actually
+ * disable the PPM decoder completely if we have S.bus signal.
*/
- unsigned rc_channels = system_state.rc_channels;
+ bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
+ if (ppm_updated)
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
- if (fds[0].revents & POLLIN)
- locked |= dsm_input();
+ ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
- if (fds[1].revents & POLLIN)
- locked |= sbus_input();
+ /*
+ * In some cases we may have received a frame, but input has still
+ * been lost.
+ */
+ bool rc_input_lost = false;
/*
- * If we don't have lock from one of the serial receivers,
- * look for PPM. It shares an input with S.bus, so there's
- * a possibility it will mis-parse an S.bus frame.
- *
- * XXX each S.bus frame will cause a PPM decoder interrupt
- * storm (lots of edges). It might be sensible to actually
- * disable the PPM decoder completely if we have an alternate
- * receiver lock.
+ * If we received a new frame from any of the RC sources, process it.
*/
- if (!locked)
- ppm_input();
+ if (dsm_updated || sbus_updated || ppm_updated) {
+
+ /* update RC-received timestamp */
+ system_state.rc_channels_timestamp = hrt_absolute_time();
+
+ /* record a bitmask of channels assigned */
+ unsigned assigned_channels = 0;
+
+ /* map raw inputs to mapped inputs */
+ /* XXX mapping should be atomic relative to protocol */
+ for (unsigned i = 0; i < r_raw_rc_count; i++) {
+
+ /* map the input channel */
+ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
+
+ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+
+ uint16_t raw = r_raw_rc_values[i];
+
+ /* implement the deadzone */
+ if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) {
+ raw += conf[PX4IO_P_RC_CONFIG_DEADZONE];
+ if (raw > conf[PX4IO_P_RC_CONFIG_CENTER])
+ raw = conf[PX4IO_P_RC_CONFIG_CENTER];
+ }
+ if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) {
+ raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE];
+ if (raw < conf[PX4IO_P_RC_CONFIG_CENTER])
+ raw = conf[PX4IO_P_RC_CONFIG_CENTER];
+ }
- /* check for manual override status */
- if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
- /* force manual input override */
- system_state.mixer_manual_override = true;
+ /* constrain to min/max values */
+ if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+ raw = conf[PX4IO_P_RC_CONFIG_MIN];
+ if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+ raw = conf[PX4IO_P_RC_CONFIG_MAX];
- } else {
- /* override not engaged, use FMU */
- system_state.mixer_manual_override = false;
+ int16_t scaled = raw;
+
+ /* adjust to zero-relative (-500..500) */
+ scaled -= 1500;
+
+ /* scale to fixed-point representation (-10000..10000) */
+ scaled *= 20;
+
+ ASSERT(scaled >= -15000);
+ ASSERT(scaled <= 15000);
+
+ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
+ scaled = -scaled;
+
+ /* and update the scaled/mapped version */
+ unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+ ASSERT(mapped < MAX_CONTROL_CHANNELS);
+
+ r_rc_values[mapped] = SIGNED_TO_REG(scaled);
+ assigned_channels |= (1 << mapped);
+ }
+ }
+
+ /* set un-assigned controls to zero */
+ for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
+ if (!(assigned_channels & (1 << i)))
+ r_rc_values[i] = 0;
+ }
+
+ /*
+ * If we got an update with zero channels, treat it as
+ * a loss of input.
+ *
+ * This might happen if a protocol-based receiver returns an update
+ * that contains no channels that we have mapped.
+ */
+ if (assigned_channels == 0) {
+ rc_input_lost = true;
+ } else {
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+ }
+
+ /*
+ * Export the valid channel bitmap
+ */
+ r_rc_valid = assigned_channels;
}
/*
* If we haven't seen any new control data in 200ms, assume we
- * have lost input and tell FMU.
+ * have lost input.
*/
if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
+ rc_input_lost = true;
- if (system_state.rc_channels > 0) {
- /*
- * If the RC signal status (lost / present) has
- * just changed, request an update immediately.
- */
- system_state.fmu_report_due = true;
- }
+ /* clear the input-kind flags here */
+ r_status_flags &= ~(
+ PX4IO_P_STATUS_FLAGS_RC_PPM |
+ PX4IO_P_STATUS_FLAGS_RC_DSM |
+ PX4IO_P_STATUS_FLAGS_RC_SBUS);
+ }
- /* set the number of channels to zero - no inputs */
- system_state.rc_channels = 0;
+ /*
+ * Handle losing RC input
+ */
+ if (rc_input_lost) {
+
+ /* Clear the RC input status flag, clear manual override flag */
+ r_status_flags &= ~(
+ PX4IO_P_STATUS_FLAGS_OVERRIDE |
+ PX4IO_P_STATUS_FLAGS_RC_OK);
+
+ /* Set the RC_LOST alarm */
+ r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
+
+ /* Mark the arrays as empty */
+ r_raw_rc_count = 0;
+ r_rc_valid = 0;
}
/*
- * PWM output updates are performed in addition on each comm update.
- * the updates here are required to ensure operation if FMU is not started
- * or stopped responding.
+ * Check for manual override.
+ *
+ * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
+ * must have R/C input.
+ * Override is enabled if either the hardcoded channel / value combination
+ * is selected, or the AP has requested it.
*/
- mixer_tick();
+ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+
+ bool override = false;
+
+ /*
+ * Check mapped channel 5; if the value is 'high' then the pilot has
+ * requested override.
+ *
+ * XXX This should be configurable.
+ */
+ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH))
+ override = true;
+
+ if (override) {
+
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
+
+ /* mix new RC input control values to servos */
+ if (dsm_updated || sbus_updated || ppm_updated)
+ mixer_tick();
+
+ } else {
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
+ }
+ }
+
}
}
-static void
-ppm_input(void)
+static bool
+ppm_input(uint16_t *values, uint16_t *num_values)
{
+ bool result = false;
+
+ /* avoid racing with PPM updates */
+ irqstate_t state = irqsave();
+
/*
- * Look for new PPM input.
+ * If we have received a new PPM frame within the last 200ms, accept it
+ * and then invalidate it.
*/
- if (ppm_last_valid_decode != 0) {
-
- /* avoid racing with PPM updates */
- irqstate_t state = irqsave();
+ if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) {
/* PPM data exists, copy it */
- system_state.rc_channels = ppm_decoded_channels;
+ *num_values = ppm_decoded_channels;
+ if (*num_values > MAX_CONTROL_CHANNELS)
+ *num_values = MAX_CONTROL_CHANNELS;
- for (unsigned i = 0; i < ppm_decoded_channels; i++) {
- system_state.rc_channel_data[i] = ppm_buffer[i];
- }
+ for (unsigned i = 0; i < *num_values; i++)
+ values[i] = ppm_buffer[i];
- /* copy the timestamp and clear it */
- system_state.rc_channels_timestamp = ppm_last_valid_decode;
+ /* clear validity */
ppm_last_valid_decode = 0;
- irqrestore(state);
-
- /* trigger an immediate report to the FMU */
- system_state.fmu_report_due = true;
+ /* good if we got any channels */
+ result = (*num_values > 0);
}
+
+ irqrestore(state);
+
+ return result;
}
diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c
index 560ef47d9..f0e8e0f32 100644
--- a/apps/px4io/dsm.c
+++ b/apps/px4io/dsm.c
@@ -43,17 +43,13 @@
#include <fcntl.h>
#include <unistd.h>
-#include <string.h>
#include <termios.h>
-#include <systemlib/ppm_decode.h>
-
#include <drivers/drv_hrt.h>
#define DEBUG
#include "px4io.h"
-#include "protocol.h"
#define DSM_FRAME_SIZE 16
#define DSM_FRAME_CHANNELS 7
@@ -72,13 +68,13 @@ unsigned dsm_frame_drops;
static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
static void dsm_guess_format(bool reset);
-static void dsm_decode(hrt_abstime now);
+static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
int
dsm_init(const char *device)
{
if (dsm_fd < 0)
- dsm_fd = open(device, O_RDONLY);
+ dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
if (dsm_fd >= 0) {
struct termios t;
@@ -106,7 +102,7 @@ dsm_init(const char *device)
}
bool
-dsm_input(void)
+dsm_input(uint16_t *values, uint16_t *num_values)
{
ssize_t ret;
hrt_abstime now;
@@ -143,7 +139,7 @@ dsm_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
- goto out;
+ return false;
last_rx_time = now;
@@ -156,20 +152,14 @@ dsm_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < DSM_FRAME_SIZE)
- goto out;
+ return false;
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
- dsm_decode(now);
partial_frame_count = 0;
-
-out:
- /*
- * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
- */
- return (now - last_frame_time) < 200000;
+ return dsm_decode(now, values, num_values);
}
static bool
@@ -273,8 +263,8 @@ dsm_guess_format(bool reset)
dsm_guess_format(true);
}
-static void
-dsm_decode(hrt_abstime frame_time)
+static bool
+dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
{
/*
@@ -295,7 +285,7 @@ dsm_decode(hrt_abstime frame_time)
/* if we don't know the frame format, update the guessing state machine */
if (channel_shift == 0) {
dsm_guess_format(false);
- return;
+ return false;
}
/*
@@ -323,8 +313,8 @@ dsm_decode(hrt_abstime frame_time)
continue;
/* update the decoded channel count */
- if (channel >= system_state.rc_channels)
- system_state.rc_channels = channel + 1;
+ if (channel >= *num_values)
+ *num_values = channel + 1;
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
if (channel_shift == 11)
@@ -355,13 +345,11 @@ dsm_decode(hrt_abstime frame_time)
break;
}
- system_state.rc_channel_data[channel] = value;
+ values[channel] = value;
}
- /* and note that we have received data from the R/C controller */
- /* XXX failsafe will cause problems here - need a strategy for detecting it */
- system_state.rc_channels_timestamp = frame_time;
-
- /* trigger an immediate report to the FMU */
- system_state.fmu_report_due = true;
+ /*
+ * XXX Note that we may be in failsafe here; we need to work out how to detect that.
+ */
+ return true;
}
diff --git a/apps/px4io/i2c.c b/apps/px4io/i2c.c
new file mode 100644
index 000000000..4485daa5b
--- /dev/null
+++ b/apps/px4io/i2c.c
@@ -0,0 +1,340 @@
+/****************************************************************************
+ *
+ * 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 i2c.c
+ *
+ * I2C communication for the PX4IO module.
+ */
+
+#include <stdint.h>
+
+#include <nuttx/arch.h>
+#include <arch/board/board.h>
+#include <stm32_i2c.h>
+#include <stm32_dma.h>
+
+//#define DEBUG
+#include "px4io.h"
+
+/*
+ * I2C register definitions.
+ */
+#define I2C_BASE STM32_I2C1_BASE
+
+#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg))
+
+#define rCR1 REG(STM32_I2C_CR1_OFFSET)
+#define rCR2 REG(STM32_I2C_CR2_OFFSET)
+#define rOAR1 REG(STM32_I2C_OAR1_OFFSET)
+#define rOAR2 REG(STM32_I2C_OAR2_OFFSET)
+#define rDR REG(STM32_I2C_DR_OFFSET)
+#define rSR1 REG(STM32_I2C_SR1_OFFSET)
+#define rSR2 REG(STM32_I2C_SR2_OFFSET)
+#define rCCR REG(STM32_I2C_CCR_OFFSET)
+#define rTRISE REG(STM32_I2C_TRISE_OFFSET)
+
+static int i2c_interrupt(int irq, void *context);
+static void i2c_rx_setup(void);
+static void i2c_tx_setup(void);
+static void i2c_rx_complete(void);
+static void i2c_tx_complete(void);
+
+static DMA_HANDLE rx_dma;
+static DMA_HANDLE tx_dma;
+
+static uint8_t rx_buf[68];
+static unsigned rx_len;
+
+static const uint8_t junk_buf[] = { 0xff, 0xff, 0xff, 0xff };
+
+static const uint8_t *tx_buf = junk_buf;
+static unsigned tx_len = sizeof(junk_buf);
+unsigned tx_count;
+
+static uint8_t selected_page;
+static uint8_t selected_offset;
+
+enum {
+ DIR_NONE = 0,
+ DIR_TX = 1,
+ DIR_RX = 2
+} direction;
+
+void
+i2c_init(void)
+{
+ debug("i2c init");
+
+ /* allocate DMA handles and initialise DMA */
+ rx_dma = stm32_dmachannel(DMACHAN_I2C1_RX);
+ i2c_rx_setup();
+ tx_dma = stm32_dmachannel(DMACHAN_I2C1_TX);
+ i2c_tx_setup();
+
+ /* enable the i2c block clock and reset it */
+ modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN);
+ modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST);
+ modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0);
+
+ /* configure the i2c GPIOs */
+ stm32_configgpio(GPIO_I2C1_SCL);
+ stm32_configgpio(GPIO_I2C1_SDA);
+
+ /* soft-reset the block */
+ rCR1 |= I2C_CR1_SWRST;
+ rCR1 = 0;
+
+ /* set for DMA operation */
+ rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
+
+ /* set the frequency value in CR2 */
+ rCR2 &= ~I2C_CR2_FREQ_MASK;
+ rCR2 |= STM32_PCLK1_FREQUENCY / 1000000;
+
+ /* set divisor and risetime for fast mode */
+ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
+ if (result < 1)
+ result = 1;
+ result = 3;
+ rCCR &= ~I2C_CCR_CCR_MASK;
+ rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
+ rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1);
+
+ /* set our device address */
+ rOAR1 = 0x1a << 1;
+
+ /* enable event interrupts */
+ irq_attach(STM32_IRQ_I2C1EV, i2c_interrupt);
+ irq_attach(STM32_IRQ_I2C1ER, i2c_interrupt);
+ up_enable_irq(STM32_IRQ_I2C1EV);
+ up_enable_irq(STM32_IRQ_I2C1ER);
+
+ /* and enable the I2C port */
+ rCR1 |= I2C_CR1_ACK | I2C_CR1_PE;
+
+#ifdef DEBUG
+ i2c_dump();
+#endif
+}
+
+
+/*
+ reset the I2C bus
+ used to recover from lockups
+ */
+void i2c_reset(void)
+{
+ rCR1 |= I2C_CR1_SWRST;
+ rCR1 = 0;
+
+ /* set for DMA operation */
+ rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
+
+ /* set the frequency value in CR2 */
+ rCR2 &= ~I2C_CR2_FREQ_MASK;
+ rCR2 |= STM32_PCLK1_FREQUENCY / 1000000;
+
+ /* set divisor and risetime for fast mode */
+ uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
+ if (result < 1)
+ result = 1;
+ result = 3;
+ rCCR &= ~I2C_CCR_CCR_MASK;
+ rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
+ rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1);
+
+ /* set our device address */
+ rOAR1 = 0x1a << 1;
+
+ /* and enable the I2C port */
+ rCR1 |= I2C_CR1_ACK | I2C_CR1_PE;
+}
+
+static int
+i2c_interrupt(int irq, FAR void *context)
+{
+ uint16_t sr1 = rSR1;
+
+ if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF | I2C_SR1_ADDR)) {
+
+ if (sr1 & I2C_SR1_STOPF) {
+ /* write to CR1 to clear STOPF */
+ (void)rSR1; /* as recommended, re-read SR1 */
+ rCR1 |= I2C_CR1_PE;
+ }
+
+ /* DMA never stops, so we should do that now */
+ switch (direction) {
+ case DIR_TX:
+ i2c_tx_complete();
+ break;
+ case DIR_RX:
+ i2c_rx_complete();
+ break;
+ default:
+ /* not currently transferring - must be a new txn */
+ break;
+ }
+ direction = DIR_NONE;
+ }
+
+ if (sr1 & I2C_SR1_ADDR) {
+
+ /* clear ADDR to ack our selection and get direction */
+ (void)rSR1; /* as recommended, re-read SR1 */
+ uint16_t sr2 = rSR2;
+
+ if (sr2 & I2C_SR2_TRA) {
+ /* we are the transmitter */
+
+ direction = DIR_TX;
+
+ } else {
+ /* we are the receiver */
+
+ direction = DIR_RX;
+ }
+ }
+
+ /* clear any errors that might need it (this handles AF as well */
+ if (sr1 & I2C_SR1_ERRORMASK)
+ rSR1 = 0;
+
+ return 0;
+}
+
+static void
+i2c_rx_setup(void)
+{
+ /*
+ * Note that we configure DMA in circular mode; this means that a too-long
+ * transfer will overwrite the buffer, but that avoids us having to deal with
+ * bailing out of a transaction while the master is still babbling at us.
+ */
+ rx_len = 0;
+ stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf),
+ DMA_CCR_CIRC |
+ DMA_CCR_MINC |
+ DMA_CCR_PSIZE_32BITS |
+ DMA_CCR_MSIZE_8BITS |
+ DMA_CCR_PRIMED);
+
+ stm32_dmastart(rx_dma, NULL, NULL, false);
+}
+
+static void
+i2c_rx_complete(void)
+{
+ rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma);
+ stm32_dmastop(rx_dma);
+
+ if (rx_len >= 2) {
+ selected_page = rx_buf[0];
+ selected_offset = rx_buf[1];
+
+ /* work out how many registers are being written */
+ unsigned count = (rx_len - 2) / 2;
+ if (count > 0) {
+ registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count);
+ } else {
+ /* no registers written, must be an address cycle */
+ uint16_t *regs;
+ unsigned reg_count;
+
+ /* work out which registers are being addressed */
+ int ret = registers_get(selected_page, selected_offset, &regs, &reg_count);
+ if (ret == 0) {
+ tx_buf = (uint8_t *)regs;
+ tx_len = reg_count * 2;
+ } else {
+ tx_buf = junk_buf;
+ tx_len = sizeof(junk_buf);
+ }
+
+ /* disable interrupts while reconfiguring DMA for the selected registers */
+ irqstate_t flags = irqsave();
+
+ stm32_dmastop(tx_dma);
+ i2c_tx_setup();
+
+ irqrestore(flags);
+ }
+ }
+
+ /* prepare for the next transaction */
+ i2c_rx_setup();
+}
+
+static void
+i2c_tx_setup(void)
+{
+ /*
+ * Note that we configure DMA in circular mode; this means that a too-long
+ * transfer will copy the buffer more than once, but that avoids us having
+ * to deal with bailing out of a transaction while the master is still
+ * babbling at us.
+ */
+ stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len,
+ DMA_CCR_DIR |
+ DMA_CCR_CIRC |
+ DMA_CCR_MINC |
+ DMA_CCR_PSIZE_8BITS |
+ DMA_CCR_MSIZE_8BITS |
+ DMA_CCR_PRIMED);
+
+ stm32_dmastart(tx_dma, NULL, NULL, false);
+}
+
+static void
+i2c_tx_complete(void)
+{
+ tx_count = tx_len - stm32_dmaresidual(tx_dma);
+ stm32_dmastop(tx_dma);
+
+ /* for debug purposes, save the length of the last transmit as seen by the DMA */
+
+ /* leave tx_buf/tx_len alone, so that a retry will see the same data */
+
+ /* prepare for the next transaction */
+ i2c_tx_setup();
+}
+
+void
+i2c_dump(void)
+{
+ debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2);
+ debug("OAR1 0x%08x OAR2 0x%08x", rOAR1, rOAR2);
+ debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE);
+ debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2);
+}
diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp
index 8e00781a0..93bd4470f 100644
--- a/apps/px4io/mixer.cpp
+++ b/apps/px4io/mixer.cpp
@@ -38,22 +38,13 @@
*/
#include <nuttx/config.h>
-#include <nuttx/arch.h>
#include <sys/types.h>
#include <stdbool.h>
#include <string.h>
-#include <assert.h>
-#include <errno.h>
-#include <unistd.h>
-#include <fcntl.h>
-#include <sched.h>
-
-#include <debug.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
-#include <drivers/device/device.h>
#include <systemlib/mixer/mixer.h>
@@ -75,13 +66,16 @@ extern "C" {
#define OVERRIDE 4
/* current servo arm/disarm state */
-bool mixer_servos_armed = false;
+static bool mixer_servos_armed = false;
/* selected control values and count for mixing */
-static uint16_t *control_values;
-static int control_count;
-
-static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
+enum mixer_source {
+ MIX_NONE,
+ MIX_FMU,
+ MIX_OVERRIDE,
+ MIX_FAILSAFE
+};
+static mixer_source source;
static int mixer_callback(uintptr_t handle,
uint8_t control_group,
@@ -93,22 +87,79 @@ static MixerGroup mixer_group(mixer_callback, 0);
void
mixer_tick(void)
{
- bool should_arm;
-
/* check that we are receiving fresh data from the FMU */
if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
- /* too many frames without FMU input, time to go to failsafe */
- system_state.mixer_manual_override = true;
- system_state.mixer_fmu_available = false;
+
+ /* too long without FMU input, time to go to failsafe */
+ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
+ debug("AP RX timeout");
+ }
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
+ r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
+ r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
+ }
+
+ source = MIX_FAILSAFE;
+
+ /*
+ * Decide which set of controls we're using.
+ */
+ if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) {
+
+ /* don't actually mix anything - we already have raw PWM values */
+ source = MIX_NONE;
+
+ } else {
+
+ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
+
+ /* mix from FMU controls */
+ source = MIX_FMU;
+ }
+
+ if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
+ (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
+
+ /* if allowed, mix from RC inputs directly */
+ source = MIX_OVERRIDE;
+ }
}
/*
- * Decide which set of inputs we're using.
+ * Run the mixers.
*/
-
- /* this is for planes, where manual override makes sense */
- if (system_state.manual_override_ok) {
+ if (source == MIX_FAILSAFE) {
+
+ /* copy failsafe values to the servo outputs */
+ for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+ r_page_servos[i] = r_page_servo_failsafe[i];
+
+ } else if (source != MIX_NONE) {
+
+ float outputs[IO_SERVO_COUNT];
+ unsigned mixed;
+
+ /* mix */
+ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
+
+ /* scale to PWM and update the servo outputs as required */
+ for (unsigned i = 0; i < mixed; i++) {
+
+ /* save actuator values for FMU readback */
+ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
+
+ /* scale to servo output */
+ r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
+
+ }
+ for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
+ r_page_servos[i] = 0;
+ }
+
+#if 0
/* if everything is ok */
+
if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
/* we have recent control data from the FMU */
control_count = PX4IO_CONTROL_CHANNELS;
@@ -169,43 +220,20 @@ mixer_tick(void)
control_count = 0;
}
}
-
- /*
- * Run the mixers if we have any control data at all.
- */
- if (control_count > 0) {
- float outputs[IO_SERVO_COUNT];
- unsigned mixed;
-
- /* mix */
- mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
-
- /* scale to PWM and update the servo outputs as required */
- for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
- if (i < mixed) {
- /* scale to servo output */
- system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
-
- } else {
- /* set to zero to inhibit PWM pulse output */
- system_state.servos[i] = 0;
- }
-
- /*
- * If we are armed, update the servo output.
- */
- if (system_state.armed && system_state.arm_ok) {
- up_pwm_servo_set(i, system_state.servos[i]);
- }
- }
- }
+#endif
/*
* Decide whether the servos should be armed right now.
- * A sufficient reason is armed state and either FMU or RC control inputs
+ *
+ * We must be armed, and we must have a PWM source; either raw from
+ * FMU or from the mixer.
+ *
+ * XXX correct behaviour for failsafe may require an additional case
+ * here.
*/
-
- should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
+ bool should_arm = (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
+ /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
+ /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)));
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
@@ -217,6 +245,12 @@ mixer_tick(void)
up_pwm_servo_arm(false);
mixer_servos_armed = false;
}
+
+ if (mixer_servos_armed) {
+ /* update the servo outputs. */
+ for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
+ up_pwm_servo_set(i, r_page_servos[i]);
+ }
}
static int
@@ -225,21 +259,40 @@ mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control)
{
- /* if the control index refers to an input that's not valid, we can't return it */
- if (control_index >= control_count)
+ if (control_group != 0)
return -1;
- /* scale from current PWM units (1000-2000) to mixer input values */
- if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) {
- control = ((float)control_values[control_index] - 1000.0f) / 1000.0f;
- } else {
- control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
+ switch (source) {
+ case MIX_FMU:
+ if (control_index < PX4IO_CONTROL_CHANNELS) {
+ control = REG_TO_FLOAT(r_page_controls[control_index]);
+ break;
+ }
+ return -1;
+
+ case MIX_OVERRIDE:
+ if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
+ control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
+ break;
+ }
+ return -1;
+
+ case MIX_FAILSAFE:
+ case MIX_NONE:
+ /* XXX we could allow for configuration of per-output failsafe values */
+ return -1;
}
return 0;
}
-static char mixer_text[256];
+/*
+ * 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;
void
@@ -260,14 +313,18 @@ mixer_handle_text(const void *buffer, size_t length)
debug("reset");
mixer_group.reset();
mixer_text_length = 0;
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
debug("append %d", length);
/* check for overflow - this is really fatal */
- if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
+ /* XXX could add just what will fit & try to parse, then repeat... */
+ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
+ }
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
@@ -281,6 +338,10 @@ mixer_handle_text(const void *buffer, size_t length)
/* if anything was parsed */
if (resid != mixer_text_length) {
+
+ /* ideally, this should test resid == 0 ? */
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
+
debug("used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h
index 90236b40c..a957a9e79 100644
--- a/apps/px4io/protocol.h
+++ b/apps/px4io/protocol.h
@@ -31,67 +31,150 @@
*
****************************************************************************/
-/**
- * @file PX4FMU <-> PX4IO messaging protocol.
- *
- * This initial version of the protocol is very simple; each side transmits a
- * complete update with each frame. This avoids the sending of many small
- * messages and the corresponding complexity involved.
- */
-
#pragma once
-#define PX4IO_CONTROL_CHANNELS 8
-#define PX4IO_INPUT_CHANNELS 12
-#define PX4IO_RELAY_CHANNELS 4
-
-#pragma pack(push, 1)
-
/**
- * Periodic command from FMU to IO.
- */
-struct px4io_command {
- uint16_t f2i_magic;
-#define F2I_MAGIC 0x636d
-
- uint16_t servo_rate;
- uint16_t output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */
- bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */
- bool arm_ok; /**< FMU allows full arming */
- bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */
- bool manual_override_ok; /**< if true, IO performs a direct manual override */
-};
-
-/**
- * Periodic report from IO to FMU
+ * @file protocol.h
+ *
+ * PX4IO I2C interface protocol.
+ *
+ * Communication is performed via writes to and reads from 16-bit virtual
+ * registers organised into pages of 255 registers each.
+ *
+ * The first two bytes of each write select a page and offset address
+ * respectively. Subsequent reads and writes increment the offset within
+ * the page.
+ *
+ * Most pages are readable or writable but not both.
+ *
+ * Note that some pages may permit offset values greater than 255, which
+ * can only be achieved by long writes. The offset does not wrap.
+ *
+ * Writes to unimplemented registers are ignored. Reads from unimplemented
+ * registers return undefined values.
+ *
+ * As convention, values that would be floating point in other parts of
+ * the PX4 system are expressed as signed integer values scaled by 10000,
+ * e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and
+ * SIGNED_TO_REG macros to convert between register representation and
+ * the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float.
+ *
+ * Note that the implementation of readable pages prefers registers within
+ * readable pages to be densely packed. Page numbers do not need to be
+ * packed.
*/
-struct px4io_report {
- uint16_t i2f_magic;
-#define I2F_MAGIC 0x7570
-
- uint16_t rc_channel[PX4IO_INPUT_CHANNELS];
- bool armed;
- uint8_t channel_count;
- uint16_t battery_mv;
- uint16_t adc_in;
- uint8_t overcurrent;
-};
-
-/**
- * As-needed config message from FMU to IO
- */
-struct px4io_config {
- uint16_t f2i_config_magic;
-#define F2I_CONFIG_MAGIC 0x6366
-
- uint8_t rc_map[4]; /**< channel ordering of roll, pitch, yaw, throttle */
- uint16_t rc_min[4]; /**< min value for each channel */
- uint16_t rc_trim[4]; /**< trim value for each channel */
- uint16_t rc_max[4]; /**< max value for each channel */
- int8_t rc_rev[4]; /**< rev value for each channel */
- uint16_t rc_dz[4]; /**< dz value for each channel */
-};
+#define PX4IO_CONTROL_CHANNELS 8
+#define PX4IO_INPUT_CHANNELS 12
+#define PX4IO_RELAY_CHANNELS 4
+
+/* Per C, this is safe for all 2's complement systems */
+#define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
+#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed))
+
+#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
+#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
+
+/* static configuration page */
+#define PX4IO_PAGE_CONFIG 0
+#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
+#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
+#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
+#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
+#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
+#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
+#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
+#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
+#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */
+
+/* dynamic status page */
+#define PX4IO_PAGE_STATUS 1
+#define PX4IO_P_STATUS_FREEMEM 0
+#define PX4IO_P_STATUS_CPULOAD 1
+
+#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
+#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
+#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
+#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */
+#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
+#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */
+#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */
+#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
+#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
+
+#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
+#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
+#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */
+#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */
+#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */
+#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */
+
+#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
+#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */
+
+/* array of post-mix actuator outputs, -10000..10000 */
+#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of PWM servo output values, microseconds */
+#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* array of raw RC input values, microseconds */
+#define PX4IO_PAGE_RAW_RC_INPUT 4
+#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
+#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
+
+/* array of scaled RC input values, -10000..10000 */
+#define PX4IO_PAGE_RC_INPUT 5
+#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */
+#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */
+
+/* array of raw ADC values */
+#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
+
+/* setup page */
+#define PX4IO_PAGE_SETUP 100
+#define PX4IO_P_SETUP_FEATURES 0
+
+#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
+#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */
+#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
+#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */
+
+#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
+#define PX4IO_P_SETUP_PWM_LOWRATE 3 /* 'low' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_PWM_HIGHRATE 4 /* 'high' PWM frame output rate in Hz */
+#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
+#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
+#define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */
+#define PX4IO_P_SETUP_IBATT_BIAS 8 /* battery current bias value */
+#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
+
+/* autopilot control values, -10000..10000 */
+#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
+
+/* raw text load to the mixer parser - ignores offset */
+#define PX4IO_PAGE_MIXERLOAD 102
+
+/* R/C channel config */
+#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
+#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
+#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
+#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
+#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */
+#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */
+#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */
+#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
+#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
+#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
+
+/* PWM output - overrides mixer */
+#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
+
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
@@ -99,18 +182,16 @@ struct px4io_config {
* This message adds text to the mixer text buffer; the text
* buffer is drained as the definitions are consumed.
*/
+#pragma pack(push, 1)
struct px4io_mixdata {
uint16_t f2i_mixer_magic;
#define F2I_MIXER_MAGIC 0x6d74
uint8_t action;
-#define F2I_MIXER_ACTION_RESET 0
-#define F2I_MIXER_ACTION_APPEND 1
+#define F2I_MIXER_ACTION_RESET 0
+#define F2I_MIXER_ACTION_APPEND 1
char text[0]; /* actual text size may vary */
};
+#pragma pack(pop)
-/* maximum size is limited by the HX frame size */
-#define F2I_MIXER_MAX_TEXT (HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata))
-
-#pragma pack(pop) \ No newline at end of file
diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c
index 88342816e..fec5eed23 100644
--- a/apps/px4io/px4io.c
+++ b/apps/px4io/px4io.c
@@ -37,20 +37,22 @@
*/
#include <nuttx/config.h>
-#include <stdio.h>
+
+#include <stdio.h> // required for task_create
#include <stdbool.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
-
-#include <nuttx/clock.h>
+#include <poll.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
+#include <systemlib/perf_counter.h>
+
+#include <stm32_uart.h>
+
+#define DEBUG
#include "px4io.h"
__EXPORT int user_start(int argc, char *argv[]);
@@ -59,6 +61,76 @@ extern void up_cxxinitialize(void);
struct sys_state_s system_state;
+static struct hrt_call serial_dma_call;
+
+/* global debug level for isr_debug() */
+volatile uint8_t debug_level = 0;
+volatile uint32_t i2c_loop_resets = 0;
+
+struct hrt_call loop_overtime_call;
+
+
+/*
+ a set of debug buffers to allow us to send debug information from ISRs
+ */
+
+static volatile uint32_t msg_counter;
+static volatile uint32_t last_msg_counter;
+static volatile uint8_t msg_next_out, msg_next_in;
+
+/*
+ * WARNING too large buffers here consume the memory required
+ * for mixer handling. Do not allocate more than 80 bytes for
+ * output.
+ */
+#define NUM_MSG 2
+static char msg[NUM_MSG][50];
+
+/*
+ add a debug message to be printed on the console
+ */
+void isr_debug(uint8_t level, const char *fmt, ...)
+{
+ if (level > debug_level) {
+ return;
+ }
+ va_list ap;
+ va_start(ap, fmt);
+ vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap);
+ va_end(ap);
+ msg_next_in = (msg_next_in+1) % NUM_MSG;
+ msg_counter++;
+}
+
+/*
+ show all pending debug messages
+ */
+static void show_debug_messages(void)
+{
+ if (msg_counter != last_msg_counter) {
+ uint32_t n = msg_counter - last_msg_counter;
+ if (n > NUM_MSG) n = NUM_MSG;
+ last_msg_counter = msg_counter;
+ while (n--) {
+ debug("%s", msg[msg_next_out]);
+ msg_next_out = (msg_next_out+1) % NUM_MSG;
+ }
+ }
+}
+
+/*
+ catch I2C lockups
+ */
+static void loop_overtime(void *arg)
+{
+ debug("RESETTING\n");
+ i2c_loop_resets++;
+ i2c_dump();
+ i2c_reset();
+ hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL);
+}
+
+
int user_start(int argc, char *argv[])
{
/* run C++ ctors before we go any further */
@@ -66,12 +138,16 @@ int user_start(int argc, char *argv[])
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
- /* default to 50 Hz PWM outputs */
- system_state.servo_rate = 50;
/* configure the high-resolution time/callout interface */
hrt_init();
+ /*
+ * Poll at 1ms intervals for received bytes that have not triggered
+ * a DMA event.
+ */
+ hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
+
/* print some startup info */
lowsyslog("\nPX4IO: starting\n");
@@ -96,10 +172,40 @@ int user_start(int argc, char *argv[])
(main_t)controls_main,
NULL);
-
struct mallinfo minfo = mallinfo();
lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
- /* we're done here, go run the communications loop */
- comms_main();
-} \ No newline at end of file
+ debug("debug_level=%u\n", (unsigned)debug_level);
+
+ /* start the i2c handler */
+ i2c_init();
+
+ /* add a performance counter for mixing */
+ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
+
+ /* run the mixer at ~300Hz (for now...) */
+ /* XXX we should use CONFIG_IDLE_CUSTOM and take over the idle thread instead of running two additional tasks */
+ uint16_t counter=0;
+ for (;;) {
+ /*
+ if we are not scheduled for 10ms then reset the I2C bus
+ */
+ hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL);
+
+ poll(NULL, 0, 3);
+ perf_begin(mixer_perf);
+ mixer_tick();
+ perf_end(mixer_perf);
+ show_debug_messages();
+ if (counter++ == 800) {
+ counter = 0;
+ isr_debug(1, "d:%u stat=0x%x arm=0x%x feat=0x%x rst=%u",
+ (unsigned)debug_level,
+ (unsigned)r_status_flags,
+ (unsigned)r_setup_arming,
+ (unsigned)r_setup_features,
+ (unsigned)i2c_loop_resets);
+ }
+ }
+}
+
diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h
index 3ce6afc31..6944776a9 100644
--- a/apps/px4io/px4io.h
+++ b/apps/px4io/px4io.h
@@ -64,129 +64,58 @@
#endif
/*
- * System state structure.
+ * Registers.
*/
-struct sys_state_s {
-
- bool armed; /* IO armed */
- bool arm_ok; /* FMU says OK to arm */
- uint16_t servo_rate; /* output rate of servos in Hz */
-
- /**
- * Remote control input(s) channel mappings
- */
- uint8_t rc_map[4];
-
- /**
- * Remote control channel attributes
- */
- uint16_t rc_min[4];
- uint16_t rc_trim[4];
- uint16_t rc_max[4];
- int16_t rc_rev[4];
- uint16_t rc_dz[4];
+extern uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */
+extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */
+extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
+extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
+extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */
+extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */
+
+extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
+extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
+extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
+extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
- /**
- * Data from the remote control input(s)
- */
- unsigned rc_channels;
- uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
- uint64_t rc_channels_timestamp;
-
- /**
- * Control signals from FMU.
- */
- uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS];
+/*
+ * Register aliases.
+ *
+ * Handy aliases for registers that are widely used.
+ */
+#define r_status_flags r_page_status[PX4IO_P_STATUS_FLAGS]
+#define r_status_alarms r_page_status[PX4IO_P_STATUS_ALARMS]
- /**
- * Mixed servo outputs
- */
- uint16_t servos[IO_SERVO_COUNT];
+#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
+#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
+#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
+#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
- /**
- * Relay controls
- */
- bool relays[PX4IO_RELAY_CHANNELS];
+#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
+#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
+#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES]
+#define r_setup_pwm_lowrate r_page_setup[PX4IO_P_SETUP_PWM_LOWRATE]
+#define r_setup_pwm_highrate r_page_setup[PX4IO_P_SETUP_PWM_HIGHRATE]
+#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS]
- /**
- * If true, we are using the FMU controls, else RC input if available.
- */
- bool mixer_manual_override;
+#define r_control_values (&r_page_controls[0])
- /**
- * If true, FMU input is available.
- */
- bool mixer_fmu_available;
+/*
+ * System state structure.
+ */
+struct sys_state_s {
- /**
- * If true, state that should be reported to FMU has been updated.
- */
- bool fmu_report_due;
+ uint64_t rc_channels_timestamp;
/**
* Last FMU receive time, in microseconds since system boot
*/
uint64_t fmu_data_received_time;
- /**
- * Current serial interface mode, per the serial_rx_mode parameter
- * in the config packet.
- */
- uint8_t serial_rx_mode;
-
- /**
- * If true, the RC signal has been lost for more than a timeout interval
- */
- bool rc_lost;
-
- /**
- * If true, the connection to FMU has been lost for more than a timeout interval
- */
- bool fmu_lost;
-
- /**
- * If true, FMU is ready for autonomous position control. Only used for LED indication
- */
- bool vector_flight_mode_ok;
-
- /**
- * If true, IO performs an on-board manual override with the RC channel values
- */
- bool manual_override_ok;
-
- /*
- * Measured battery voltage in mV
- */
- uint16_t battery_mv;
-
- /*
- * ADC IN5 measurement
- */
- uint16_t adc_in5;
-
- /*
- * Power supply overcurrent status bits.
- */
- uint8_t overcurrent;
-
};
extern struct sys_state_s system_state;
-#if 0
-/*
- * Software countdown timers.
- *
- * Each timer counts down to zero at one tick per ms.
- */
-#define TIMER_BLINK_AMBER 0
-#define TIMER_BLINK_BLUE 1
-#define TIMER_STATUS_PRINT 2
-#define TIMER_SANITY 7
-#define TIMER_NUM_TIMERS 8
-extern volatile int timers[TIMER_NUM_TIMERS];
-#endif
-
/*
* GPIO handling.
*/
@@ -206,6 +135,7 @@ extern volatile int timers[TIMER_NUM_TIMERS];
#define ADC_VBATT 4
#define ADC_IN5 5
+#define ADC_CHANNEL_COUNT 2
/*
* Mixer
@@ -222,6 +152,13 @@ extern void safety_init(void);
* FMU communications
*/
extern void comms_main(void) __attribute__((noreturn));
+extern void i2c_init(void);
+
+/*
+ * Register space
+ */
+extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
+extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
/*
* Sensors/misc inputs
@@ -231,16 +168,21 @@ extern uint16_t adc_measure(unsigned channel);
/*
* R/C receiver handling.
+ *
+ * Input functions return true when they receive an update from the RC controller.
*/
extern void controls_main(void);
extern int dsm_init(const char *device);
-extern bool dsm_input(void);
+extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern int sbus_init(const char *device);
-extern bool sbus_input(void);
+extern bool sbus_input(uint16_t *values, uint16_t *num_values);
+
+/* global debug level for isr_debug() */
+extern volatile uint8_t debug_level;
+
+/* send a debug message to the console */
+extern void isr_debug(uint8_t level, const char *fmt, ...);
+
+void i2c_dump(void);
+void i2c_reset(void);
-/*
- * Assertion codes
- */
-#define A_GPIO_OPEN_FAIL 100
-#define A_SERVO_OPEN_FAIL 101
-#define A_INPUTQ_OPEN_FAIL 102
diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c
new file mode 100644
index 000000000..40bf72482
--- /dev/null
+++ b/apps/px4io/registers.c
@@ -0,0 +1,567 @@
+/****************************************************************************
+ *
+ * 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 registers.c
+ *
+ * Implementation of the PX4IO register space.
+ */
+
+#include <nuttx/config.h>
+
+#include <stdbool.h>
+#include <stdlib.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "px4io.h"
+#include "protocol.h"
+
+static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
+
+/**
+ * PAGE 0
+ *
+ * Static configuration parameters.
+ */
+static const uint16_t r_page_config[] = {
+ [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */
+ [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */
+ [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */
+ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
+ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
+ [PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
+ [PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS,
+ [PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT,
+ [PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
+};
+
+/**
+ * PAGE 1
+ *
+ * Status values.
+ */
+uint16_t r_page_status[] = {
+ [PX4IO_P_STATUS_FREEMEM] = 0,
+ [PX4IO_P_STATUS_CPULOAD] = 0,
+ [PX4IO_P_STATUS_FLAGS] = 0,
+ [PX4IO_P_STATUS_ALARMS] = 0,
+ [PX4IO_P_STATUS_VBATT] = 0,
+ [PX4IO_P_STATUS_IBATT] = 0
+};
+
+/**
+ * PAGE 2
+ *
+ * Post-mixed actuator values.
+ */
+uint16_t r_page_actuators[IO_SERVO_COUNT];
+
+/**
+ * PAGE 3
+ *
+ * Servo PWM values
+ */
+uint16_t r_page_servos[IO_SERVO_COUNT];
+
+/**
+ * PAGE 4
+ *
+ * Raw RC input
+ */
+uint16_t r_page_raw_rc_input[] =
+{
+ [PX4IO_P_RAW_RC_COUNT] = 0,
+ [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+};
+
+/**
+ * PAGE 5
+ *
+ * Scaled/routed RC input
+ */
+uint16_t r_page_rc_input[] = {
+ [PX4IO_P_RC_VALID] = 0,
+ [PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
+};
+
+/**
+ * PAGE 6
+ *
+ * Raw ADC input.
+ */
+uint16_t r_page_adc[ADC_CHANNEL_COUNT];
+
+/**
+ * PAGE 100
+ *
+ * Setup registers
+ */
+volatile uint16_t r_page_setup[] =
+{
+ [PX4IO_P_SETUP_FEATURES] = 0,
+ [PX4IO_P_SETUP_ARMING] = 0,
+ [PX4IO_P_SETUP_PWM_RATES] = 0,
+ [PX4IO_P_SETUP_PWM_LOWRATE] = 50,
+ [PX4IO_P_SETUP_PWM_HIGHRATE] = 200,
+ [PX4IO_P_SETUP_RELAYS] = 0,
+ [PX4IO_P_SETUP_VBATT_SCALE] = 10000,
+ [PX4IO_P_SETUP_IBATT_SCALE] = 0,
+ [PX4IO_P_SETUP_IBATT_BIAS] = 0
+};
+
+#define PX4IO_P_SETUP_FEATURES_VALID (0)
+#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \
+ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK)
+#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
+#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
+
+/**
+ * PAGE 101
+ *
+ * Control values from the FMU.
+ */
+volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
+
+/*
+ * PAGE 102 does not have a buffer.
+ */
+
+/**
+ * PAGE 103
+ *
+ * R/C channel input configuration.
+ */
+uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
+
+/* valid options excluding ENABLE */
+#define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE
+
+/*
+ * PAGE 104 uses r_page_servos.
+ */
+
+/**
+ * PAGE 105
+ *
+ * Failsafe servo PWM values
+ */
+uint16_t r_page_servo_failsafe[IO_SERVO_COUNT];
+
+void
+registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
+{
+
+ switch (page) {
+
+ /* handle bulk controls input */
+ case PX4IO_PAGE_CONTROLS:
+
+ /* copy channel data */
+ while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+ /* XXX range-check value? */
+ r_page_controls[offset] = *values;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+
+ /* XXX we should cause a mixer tick ASAP */
+ system_state.fmu_data_received_time = hrt_absolute_time();
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
+ break;
+
+ /* handle raw PWM input */
+ case PX4IO_PAGE_DIRECT_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+ /* XXX range-check value? */
+ r_page_servos[offset] = *values;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+
+ /* XXX we should cause a mixer tick ASAP */
+ system_state.fmu_data_received_time = hrt_absolute_time();
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
+ break;
+
+ /* handle setup for servo failsafe values */
+ case PX4IO_PAGE_FAILSAFE_PWM:
+
+ /* copy channel data */
+ while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
+
+ /* XXX range-check value? */
+ r_page_servo_failsafe[offset] = *values;
+
+ offset++;
+ num_values--;
+ values++;
+ }
+ break;
+
+ /* handle text going to the mixer parser */
+ case PX4IO_PAGE_MIXERLOAD:
+ mixer_handle_text(values, num_values * sizeof(*values));
+ break;
+
+ default:
+ /* avoid offset wrap */
+ if ((offset + num_values) > 255)
+ num_values = 255 - offset;
+
+ /* iterate individual registers, set each in turn */
+ while (num_values--) {
+ if (registers_set_one(page, offset, *values))
+ break;
+ offset++;
+ values++;
+ }
+ }
+}
+
+static int
+registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
+{
+ switch (page) {
+
+ case PX4IO_PAGE_STATUS:
+ switch (offset) {
+ case PX4IO_P_STATUS_ALARMS:
+ /* clear bits being written */
+ r_status_alarms &= ~value;
+ break;
+
+ case PX4IO_P_STATUS_FLAGS:
+ /*
+ * Allow FMU override of arming state (to allow in-air restores),
+ * but only if the arming state is not in sync on the IO side.
+ */
+ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
+ r_status_flags = value;
+ }
+ break;
+
+ default:
+ /* just ignore writes to other registers in this page */
+ break;
+ }
+ break;
+
+ case PX4IO_PAGE_SETUP:
+ switch (offset) {
+ case PX4IO_P_SETUP_FEATURES:
+
+ value &= PX4IO_P_SETUP_FEATURES_VALID;
+ r_setup_features = value;
+
+ /* no implemented feature selection at this point */
+
+ break;
+
+ case PX4IO_P_SETUP_ARMING:
+
+ value &= PX4IO_P_SETUP_ARMING_VALID;
+
+ /*
+ * Update arming state - disarm if no longer OK.
+ * This builds on the requirement that the FMU driver
+ * asks about the FMU arming state on initialization,
+ * so that an in-air reset of FMU can not lead to a
+ * lockup of the IO arming state.
+ */
+ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
+ }
+
+ r_setup_arming = value;
+
+ break;
+
+ case PX4IO_P_SETUP_PWM_RATES:
+ value &= PX4IO_P_SETUP_RATES_VALID;
+ r_setup_pwm_rates = value;
+ /* XXX re-configure timers */
+ break;
+
+ case PX4IO_P_SETUP_PWM_LOWRATE:
+ if (value < 50)
+ value = 50;
+ if (value > 400)
+ value = 400;
+ r_setup_pwm_lowrate = value;
+ /* XXX re-configure timers */
+ break;
+
+ case PX4IO_P_SETUP_PWM_HIGHRATE:
+ if (value < 50)
+ value = 50;
+ if (value > 400)
+ value = 400;
+ r_setup_pwm_highrate = value;
+ /* XXX re-configure timers */
+ break;
+
+ case PX4IO_P_SETUP_RELAYS:
+ value &= PX4IO_P_SETUP_RELAYS_VALID;
+ r_setup_relays = value;
+ POWER_RELAY1(value & (1 << 0) ? 1 : 0);
+ POWER_RELAY2(value & (1 << 1) ? 1 : 0);
+ POWER_ACC1(value & (1 << 2) ? 1 : 0);
+ POWER_ACC2(value & (1 << 3) ? 1 : 0);
+ break;
+
+ case PX4IO_P_SETUP_SET_DEBUG:
+ debug_level = value;
+ isr_debug(0, "set debug %u\n", (unsigned)debug_level);
+ break;
+
+ default:
+ return -1;
+ }
+ break;
+
+ case PX4IO_PAGE_RC_CONFIG: {
+ unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
+ unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
+ uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
+
+ if (channel >= MAX_CONTROL_CHANNELS)
+ return -1;
+
+ /* disable the channel until we have a chance to sanity-check it */
+ conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+
+ switch (index) {
+
+ case PX4IO_P_RC_CONFIG_MIN:
+ case PX4IO_P_RC_CONFIG_CENTER:
+ case PX4IO_P_RC_CONFIG_MAX:
+ case PX4IO_P_RC_CONFIG_DEADZONE:
+ case PX4IO_P_RC_CONFIG_ASSIGNMENT:
+ conf[index] = value;
+ break;
+
+ case PX4IO_P_RC_CONFIG_OPTIONS:
+ value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
+
+ /* set all options except the enabled option */
+ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+
+ /* should the channel be enabled? */
+ /* this option is normally set last */
+ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
+ /* assert min..center..max ordering */
+ if (conf[PX4IO_P_RC_CONFIG_MIN] < 500)
+ break;
+ if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500)
+ break;
+ if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN])
+ break;
+ if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX])
+ break;
+ /* assert deadzone is sane */
+ if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500)
+ break;
+ if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]))
+ break;
+ if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]))
+ break;
+ if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS)
+ break;
+
+ /* sanity checks pass, enable channel */
+ conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+ }
+ break;
+ /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
+
+ }
+ break;
+ /* case PX4IO_RC_PAGE_CONFIG */
+ }
+
+ default:
+ return -1;
+ }
+ return 0;
+}
+
+uint8_t last_page;
+uint8_t last_offset;
+
+int
+registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
+{
+#define SELECT_PAGE(_page_name) \
+ do { \
+ *values = &_page_name[0]; \
+ *num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
+ } while(0)
+
+ switch (page) {
+
+ /*
+ * Handle pages that are updated dynamically at read time.
+ */
+ case PX4IO_PAGE_STATUS:
+ /* PX4IO_P_STATUS_FREEMEM */
+ {
+ struct mallinfo minfo = mallinfo();
+ r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
+ }
+
+ /* XXX PX4IO_P_STATUS_CPULOAD */
+
+ /* PX4IO_P_STATUS_FLAGS maintained externally */
+
+ /* PX4IO_P_STATUS_ALARMS maintained externally */
+
+ /* PX4IO_P_STATUS_VBATT */
+ {
+ /*
+ * Coefficients here derived by measurement of the 5-16V
+ * range on one unit:
+ *
+ * V counts
+ * 5 1001
+ * 6 1219
+ * 7 1436
+ * 8 1653
+ * 9 1870
+ * 10 2086
+ * 11 2303
+ * 12 2522
+ * 13 2738
+ * 14 2956
+ * 15 3172
+ * 16 3389
+ *
+ * slope = 0.0046067
+ * intercept = 0.3863
+ *
+ * Intercept corrected for best results @ 12V.
+ */
+ unsigned counts = adc_measure(ADC_VBATT);
+ unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+ unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
+
+ r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
+ }
+
+ /* PX4IO_P_STATUS_IBATT */
+ {
+ unsigned counts = adc_measure(ADC_VBATT);
+ unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
+ int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
+ if (corrected < 0)
+ corrected = 0;
+ r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
+ }
+
+ SELECT_PAGE(r_page_status);
+ break;
+
+ case PX4IO_PAGE_RAW_ADC_INPUT:
+ r_page_adc[0] = adc_measure(ADC_VBATT);
+ r_page_adc[1] = adc_measure(ADC_IN5);
+
+ SELECT_PAGE(r_page_adc);
+ break;
+
+ /*
+ * Pages that are just a straight read of the register state.
+ */
+
+ /* status pages */
+ case PX4IO_PAGE_CONFIG:
+ SELECT_PAGE(r_page_config);
+ break;
+ case PX4IO_PAGE_ACTUATORS:
+ SELECT_PAGE(r_page_actuators);
+ break;
+ case PX4IO_PAGE_SERVOS:
+ SELECT_PAGE(r_page_servos);
+ break;
+ case PX4IO_PAGE_RAW_RC_INPUT:
+ SELECT_PAGE(r_page_raw_rc_input);
+ break;
+ case PX4IO_PAGE_RC_INPUT:
+ SELECT_PAGE(r_page_rc_input);
+ break;
+
+ /* readback of input pages */
+ case PX4IO_PAGE_SETUP:
+ SELECT_PAGE(r_page_setup);
+ break;
+ case PX4IO_PAGE_CONTROLS:
+ SELECT_PAGE(r_page_controls);
+ break;
+ case PX4IO_PAGE_RC_CONFIG:
+ SELECT_PAGE(r_page_rc_input_config);
+ break;
+ case PX4IO_PAGE_DIRECT_PWM:
+ SELECT_PAGE(r_page_servos);
+ break;
+ case PX4IO_PAGE_FAILSAFE_PWM:
+ SELECT_PAGE(r_page_servo_failsafe);
+ break;
+
+ default:
+ return -1;
+ }
+
+#undef SELECT_PAGE
+#undef COPY_PAGE
+
+last_page = page;
+last_offset = offset;
+
+ /* if the offset is at or beyond the end of the page, we have no data */
+ if (offset >= *num_values)
+ return -1;
+
+ /* correct the data pointer and count for the offset */
+ *values += offset;
+ *num_values -= offset;
+
+ return 0;
+}
diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c
index 0cae29ac9..540636e19 100644
--- a/apps/px4io/safety.c
+++ b/apps/px4io/safety.c
@@ -36,15 +36,8 @@
*/
#include <nuttx/config.h>
-#include <stdio.h>
-#include <stdbool.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <debug.h>
-#include <stdlib.h>
-#include <errno.h>
-#include <nuttx/clock.h>
+#include <stdbool.h>
#include <drivers/drv_hrt.h>
@@ -116,30 +109,28 @@ safety_check_button(void *arg)
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
- if (safety_button_pressed && !system_state.armed) {
+ if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
- /* change to armed state and notify the FMU */
- system_state.armed = true;
+ /* switch to armed state */
+ r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
counter++;
- system_state.fmu_report_due = true;
}
/* Disarm quickly */
- } else if (safety_button_pressed && system_state.armed) {
+ } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
- system_state.armed = false;
+ r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
counter++;
- system_state.fmu_report_due = true;
}
} else {
@@ -149,18 +140,18 @@ safety_check_button(void *arg)
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE;
- if (system_state.armed) {
- if (system_state.arm_ok) {
+ if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
+ if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
pattern = LED_PATTERN_IO_FMU_ARMED;
} else {
pattern = LED_PATTERN_IO_ARMED;
}
- } else if (system_state.arm_ok) {
+ } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
pattern = LED_PATTERN_FMU_ARMED;
- } else if (system_state.vector_flight_mode_ok) {
+ } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
}
@@ -188,7 +179,7 @@ failsafe_blink(void *arg)
static bool failsafe = false;
/* blink the failsafe LED if we don't have FMU input */
- if (!system_state.mixer_fmu_available) {
+ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
failsafe = !failsafe;
} else {
diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c
index 568ef8091..d199a9361 100644
--- a/apps/px4io/sbus.c
+++ b/apps/px4io/sbus.c
@@ -66,13 +66,13 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
-static void sbus_decode(hrt_abstime frame_time);
+static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
int
sbus_init(const char *device)
{
if (sbus_fd < 0)
- sbus_fd = open(device, O_RDONLY);
+ sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
if (sbus_fd >= 0) {
struct termios t;
@@ -97,7 +97,7 @@ sbus_init(const char *device)
}
bool
-sbus_input(void)
+sbus_input(uint16_t *values, uint16_t *num_values)
{
ssize_t ret;
hrt_abstime now;
@@ -134,7 +134,7 @@ sbus_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
- goto out;
+ return false;
last_rx_time = now;
@@ -147,20 +147,14 @@ sbus_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < SBUS_FRAME_SIZE)
- goto out;
+ return false;
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
- sbus_decode(now);
partial_frame_count = 0;
-
-out:
- /*
- * If we have seen a frame in the last 200ms, we consider ourselves 'locked'
- */
- return (now - last_frame_time) < 200000;
+ return sbus_decode(now, values, num_values);
}
/*
@@ -199,13 +193,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
};
-static void
-sbus_decode(hrt_abstime frame_time)
+static bool
+sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
sbus_frame_drops++;
- return;
+ return false;
}
/* if the failsafe or connection lost bit is set, we consider the frame invalid */
@@ -213,8 +207,8 @@ sbus_decode(hrt_abstime frame_time)
(frame[23] & (1 << 3))) { /* failsafe */
/* actively announce signal loss */
- system_state.rc_channels = 0;
- return 1;
+ *values = 0;
+ return false;
}
/* we have received something we think is a frame */
@@ -241,23 +235,19 @@ sbus_decode(hrt_abstime frame_time)
}
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
- system_state.rc_channel_data[channel] = (value / 2) + 998;
+ values[channel] = (value / 2) + 998;
}
/* decode switch channels if data fields are wide enough */
if (chancount > 17) {
/* channel 17 (index 16) */
- system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998;
+ values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
- system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998;
+ values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
}
/* note the number of channels decoded */
- system_state.rc_channels = chancount;
-
- /* and note that we have received data from the R/C controller */
- system_state.rc_channels_timestamp = frame_time;
+ *num_values = chancount;
- /* trigger an immediate report to the FMU */
- system_state.fmu_report_due = true;
+ return true;
}
diff --git a/apps/systemcmds/i2c/Makefile b/apps/systemcmds/i2c/Makefile
new file mode 100644
index 000000000..046e74757
--- /dev/null
+++ b/apps/systemcmds/i2c/Makefile
@@ -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.
+#
+############################################################################
+
+#
+# Build the i2c test tool.
+#
+
+APPNAME = i2c
+PRIORITY = SCHED_PRIORITY_DEFAULT
+STACKSIZE = 4096
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/systemcmds/i2c/i2c.c b/apps/systemcmds/i2c/i2c.c
new file mode 100644
index 000000000..e1babdc12
--- /dev/null
+++ b/apps/systemcmds/i2c/i2c.c
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 i2c.c
+ *
+ * i2c hacking tool
+ */
+
+#include <nuttx/config.h>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/i2c.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/systemlib.h"
+#include "systemlib/err.h"
+
+#ifndef PX4_I2C_BUS_ONBOARD
+# error PX4_I2C_BUS_ONBOARD not defined, no device interface
+#endif
+#ifndef PX4_I2C_OBDEV_PX4IO
+# error PX4_I2C_OBDEV_PX4IO not defined
+#endif
+
+__EXPORT int i2c_main(int argc, char *argv[]);
+
+static int transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
+
+static struct i2c_dev_s *i2c;
+
+int i2c_main(int argc, char *argv[])
+{
+ /* find the right I2C */
+ i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
+
+ if (i2c == NULL)
+ errx(1, "failed to locate I2C bus");
+
+ usleep(100000);
+
+ uint8_t buf[] = { 0, 4};
+ int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0);
+
+ if (ret)
+ errx(1, "send failed - %d", ret);
+
+ uint32_t val;
+ ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val));
+ if (ret)
+ errx(1, "recive failed - %d", ret);
+
+ errx(0, "got 0x%08x", val);
+}
+
+static int
+transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
+{
+ struct i2c_msg_s msgv[2];
+ unsigned msgs;
+ int ret;
+
+ // debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
+
+ msgs = 0;
+
+ if (send_len > 0) {
+ msgv[msgs].addr = address;
+ msgv[msgs].flags = 0;
+ msgv[msgs].buffer = send;
+ msgv[msgs].length = send_len;
+ msgs++;
+ }
+
+ if (recv_len > 0) {
+ msgv[msgs].addr = address;
+ msgv[msgs].flags = I2C_M_READ;
+ msgv[msgs].buffer = recv;
+ msgv[msgs].length = recv_len;
+ msgs++;
+ }
+
+ if (msgs == 0)
+ return -1;
+
+ /*
+ * I2C architecture means there is an unavoidable race here
+ * if there are any devices on the bus with a different frequency
+ * preference. Really, this is pointless.
+ */
+ I2C_SETFREQUENCY(i2c, 400000);
+ ret = I2C_TRANSFER(i2c, &msgv[0], msgs);
+
+ // reset the I2C bus to unwedge on error
+ if (ret != OK)
+ up_i2creset(i2c);
+
+ return ret;
+}
diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h
index 40278c56c..088c4fc8f 100644
--- a/apps/uORB/topics/actuator_controls_effective.h
+++ b/apps/uORB/topics/actuator_controls_effective.h
@@ -37,8 +37,8 @@
* Actuator control topics - mixer inputs.
*
* Values published to these topics are the outputs of the vehicle control
- * system, and are expected to be mixed and used to drive the actuators
- * (servos, speed controls, etc.) that operate the vehicle.
+ * system and mixing process; they are the control-scale values that are
+ * then fed to the actual actuator driver.
*
* Each topic can be published by a single controller
*/
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 06b4c5ca5..893e34537 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -156,7 +156,9 @@ struct vehicle_status_s
enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */
enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */
- int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */
+ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
+ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
+ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
/* system flags - these represent the state predicates */
@@ -171,7 +173,7 @@ struct vehicle_status_s
bool flag_control_position_enabled; /**< true if position is controlled */
bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
- bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
+ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
bool flag_preflight_accel_calibration;
bool rc_signal_found_once;
@@ -209,6 +211,7 @@ struct vehicle_status_s
bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
bool flag_valid_launch_position; /**< indicates a valid launch position */
+ bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */
};
/**
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-03 09:15:00 +0000