diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-14 10:38:34 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-14 10:38:34 +0200 |
commit | 51a4ef5de1bc542ac4f7072d95250cd62ea73ed6 (patch) | |
tree | b71db4faea6a0ac39e4fa28481421a2acc13a896 /src/drivers/px4io | |
parent | 5e0911046173e01a6c66b91d3e38212e093159d0 (diff) | |
parent | ddc8f1fa5f5b88549af5e4f5f46c751a5f3af3ce (diff) | |
download | px4-firmware-sbus2_sensors.tar.gz px4-firmware-sbus2_sensors.tar.bz2 px4-firmware-sbus2_sensors.zip |
merged upstream/master into sbus2_sensorssbus2_sensors
Diffstat (limited to 'src/drivers/px4io')
-rw-r--r-- | src/drivers/px4io/module.mk | 2 | ||||
-rw-r--r-- | src/drivers/px4io/px4io.cpp | 207 | ||||
-rwxr-xr-x[-rw-r--r--] | src/drivers/px4io/px4io_i2c.cpp | 2 | ||||
-rw-r--r-- | src/drivers/px4io/px4io_serial.cpp | 2 | ||||
-rw-r--r-- | src/drivers/px4io/px4io_uploader.cpp | 37 |
5 files changed, 188 insertions, 62 deletions
diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk index 2054faa12..c14f1f783 100644 --- a/src/drivers/px4io/module.mk +++ b/src/drivers/px4io/module.mk @@ -44,3 +44,5 @@ SRCS = px4io.cpp \ # XXX prune to just get UART registers INCLUDE_DIRS += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common + +MODULE_STACKSIZE = 1200 diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 7c7b3dcb7..7d78b0d27 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -72,6 +72,7 @@ #include <systemlib/systemlib.h> #include <systemlib/scheduling_priorities.h> #include <systemlib/param/param.h> +#include <systemlib/circuit_breaker.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/actuator_outputs.h> @@ -91,6 +92,8 @@ #include "uploader.h" +#include "modules/dataman/dataman.h" + extern device::Device *PX4IO_i2c_interface() weak_function; extern device::Device *PX4IO_serial_interface() weak_function; @@ -195,8 +198,10 @@ public: * Print IO status. * * Print all relevant IO status information + * + * @param extended_status Shows more verbose information (in particular RC config) */ - void print_status(); + void print_status(bool extended_status); /** * Fetch and print debug console output. @@ -527,6 +532,11 @@ PX4IO::~PX4IO() if (_interface != nullptr) delete _interface; + /* deallocate perfs */ + perf_free(_perf_update); + perf_free(_perf_write); + perf_free(_perf_chan_count); + g_dev = nullptr; } @@ -568,9 +578,17 @@ int PX4IO::init() { int ret; + param_t sys_restart_param; + int sys_restart_val = DM_INIT_REASON_VOLATILE; ASSERT(_task == -1); + sys_restart_param = param_find("SYS_RESTART_TYPE"); + if (sys_restart_param != PARAM_INVALID) { + /* Indicate restart type is unknown */ + param_set(sys_restart_param, &sys_restart_val); + } + /* do regular cdev init */ ret = CDev::init(); @@ -675,6 +693,25 @@ PX4IO::init() /* send command to arm system via command API */ vehicle_command_s cmd; + /* send this to itself */ + param_t sys_id_param = param_find("MAV_SYS_ID"); + param_t comp_id_param = param_find("MAV_COMP_ID"); + + int32_t sys_id; + int32_t comp_id; + + if (param_get(sys_id_param, &sys_id)) { + errx(1, "PRM SYSID"); + } + + if (param_get(comp_id_param, &comp_id)) { + errx(1, "PRM CMPID"); + } + + cmd.target_system = sys_id; + cmd.target_component = comp_id; + cmd.source_system = sys_id; + cmd.source_component = comp_id; /* request arming */ cmd.param1 = 1.0f; cmd.param2 = 0; @@ -684,10 +721,7 @@ PX4IO::init() 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; @@ -720,6 +754,11 @@ PX4IO::init() /* keep waiting for state change for 2 s */ } while (!safety.armed); + /* Indicate restart type is in-flight */ + sys_restart_val = DM_INIT_REASON_IN_FLIGHT; + param_set(sys_restart_param, &sys_restart_val); + + /* regular boot, no in-air restart, init IO */ } else { @@ -745,6 +784,10 @@ PX4IO::init() } } + /* Indicate restart type is power on */ + sys_restart_val = DM_INIT_REASON_POWER_ON; + param_set(sys_restart_param, &sys_restart_val); + } /* try to claim the generic PWM output device node as well - it's OK if we fail at this */ @@ -756,7 +799,12 @@ PX4IO::init() } /* start the IO interface task */ - _task = task_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 2048, (main_t)&PX4IO::task_main_trampoline, nullptr); + _task = task_spawn_cmd("px4io", + SCHED_DEFAULT, + SCHED_PRIORITY_ACTUATOR_OUTPUTS, + 2000, + (main_t)&PX4IO::task_main_trampoline, + nullptr); if (_task < 0) { debug("task start failed: %d", errno); @@ -944,8 +992,40 @@ PX4IO::task_main() int pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE, &scaling, 1); if (pret != OK) { - log("voltage scaling upload failed"); + log("vscale upload failed"); + } + + /* send RC throttle failsafe value to IO */ + int32_t failsafe_param_val; + param_t failsafe_param = param_find("RC_FAILS_THR"); + + if (failsafe_param != PARAM_INVALID) { + + param_get(failsafe_param, &failsafe_param_val); + + if (failsafe_param_val > 0) { + + uint16_t failsafe_thr = failsafe_param_val; + pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1); + if (pret != OK) { + log("failsafe upload failed, FS: %d us", (int)failsafe_thr); + } + } + } + + int32_t safety_param_val; + param_t safety_param = param_find("RC_FAILS_THR"); + + if (safety_param != PARAM_INVALID) { + + param_get(safety_param, &safety_param_val); + + if (safety_param_val == PX4IO_FORCE_SAFETY_MAGIC) { + /* disable IO safety if circuit breaker asked for it */ + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, safety_param_val); + } } + } } @@ -1303,7 +1383,7 @@ void PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) { /* only publish if battery has a valid minimum voltage */ - if (vbatt <= 3300) { + if (vbatt <= 4900) { return; } @@ -1332,12 +1412,15 @@ PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) battery_status.discharged_mah = _battery_mamphour_total; _battery_last_timestamp = battery_status.timestamp; - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + /* the announced battery status would conflict with the simulated battery status in HIL */ + if (!(_pub_blocked)) { + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } } } @@ -1397,7 +1480,7 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) /* we don't have the status bits, so input_source has to be set elsewhere */ input_rc.input_source = RC_INPUT_SOURCE_UNKNOWN; - static const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); + const unsigned prolog = (PX4IO_P_RAW_RC_BASE - PX4IO_P_RAW_RC_COUNT); uint16_t regs[RC_INPUT_MAX_CHANNELS + prolog]; /* @@ -1405,8 +1488,6 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * * This should be the common case (9 channel R/C control being a reasonable upper bound). */ - input_rc.timestamp_publication = hrt_absolute_time(); - ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT, ®s[0], prolog + 9); if (ret != OK) @@ -1418,23 +1499,38 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) */ channel_count = regs[PX4IO_P_RAW_RC_COUNT]; - if (channel_count != _rc_chan_count) + /* limit the channel count */ + if (channel_count > RC_INPUT_MAX_CHANNELS) { + channel_count = RC_INPUT_MAX_CHANNELS; + } + + /* count channel count changes to identify signal integrity issues */ + if (channel_count != _rc_chan_count) { perf_count(_perf_chan_count); + } _rc_chan_count = channel_count; + input_rc.timestamp_publication = hrt_absolute_time(); + input_rc.rc_ppm_frame_length = regs[PX4IO_P_RAW_RC_DATA]; input_rc.rssi = regs[PX4IO_P_RAW_RC_NRSSI]; input_rc.rc_failsafe = (regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + input_rc.rc_lost = !(regs[PX4IO_P_RAW_RC_FLAGS] & PX4IO_P_RAW_RC_FLAGS_RC_OK); input_rc.rc_lost_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; + input_rc.channel_count = channel_count; /* rc_lost has to be set before the call to this function */ - if (!input_rc.rc_lost && !input_rc.rc_failsafe) + if (!input_rc.rc_lost && !input_rc.rc_failsafe) { _rc_last_valid = input_rc.timestamp_publication; + } input_rc.timestamp_last_signal = _rc_last_valid; + /* FIELDS NOT SET HERE */ + /* input_rc.input_source is set after this call XXX we might want to mirror the flags in the RC struct */ + if (channel_count > 9) { ret = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + 9, ®s[prolog + 9], channel_count - 9); @@ -1442,8 +1538,10 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) return ret; } - input_rc.channel_count = channel_count; - memcpy(input_rc.values, ®s[prolog], channel_count * 2); + /* last thing set are the actual channel values as 16 bit values */ + for (unsigned i = 0; i < channel_count; i++) { + input_rc.values[i] = regs[prolog + i]; + } return ret; } @@ -1476,10 +1574,11 @@ PX4IO::io_publish_raw_rc() } else { rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; - /* we do not know the RC input, only publish if RC OK flag is set */ - /* if no raw RC, just don't publish */ - if (!(_status & PX4IO_P_STATUS_FLAGS_RC_OK)) + /* only keep publishing RC input if we ever got a valid input */ + if (_rc_last_valid == 0) { + /* we have never seen valid RC signals, abort */ return OK; + } } /* lazily advertise on first publication */ @@ -1767,7 +1866,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } void -PX4IO::print_status() +PX4IO::print_status(bool extended_status) { /* basic configuration */ printf("protocol %u hardware %u bootloader %u buffer %uB crc 0x%04x%04x\n", @@ -1921,26 +2020,30 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); #endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - printf("controls"); - - for (unsigned i = 0; i < _max_controls; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); - - printf("\n"); - - for (unsigned i = 0; i < _max_rc_input; i++) { - unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; - uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); - printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", - i, - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), - io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), - options, - ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), - ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + for (unsigned group = 0; group < 4; group++) { + printf("controls %u:", group); + + for (unsigned i = 0; i < _max_controls; i++) + printf(" %d", (int16_t) io_reg_get(PX4IO_PAGE_CONTROLS, group * PX4IO_PROTOCOL_MAX_CONTROL_COUNT + i)); + + printf("\n"); + } + + if (extended_status) { + for (unsigned i = 0; i < _max_rc_input; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); + printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + } } printf("failsafe"); @@ -1957,8 +2060,7 @@ PX4IO::print_status() } int -PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) -/* Make it obvious that file * isn't used here */ +PX4IO::ioctl(file * filep, int cmd, unsigned long arg) { int ret = OK; @@ -2109,6 +2211,10 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case PWM_SERVO_GET_DISABLE_LOCKDOWN: *(unsigned *)arg = _lockdown_override; + + case PWM_SERVO_SET_FORCE_SAFETY_OFF: + /* force safety swith off */ + ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC); break; case DSM_BIND_START: @@ -2370,8 +2476,9 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; default: - /* not a recognized value */ - ret = -ENOTTY; + /* see if the parent class can make any use of it */ + ret = CDev::ioctl(filep, cmd, arg); + break; } return ret; @@ -2764,7 +2871,7 @@ monitor(void) if (g_dev != nullptr) { printf("\033[2J\033[H"); /* move cursor home and clear screen */ - (void)g_dev->print_status(); + (void)g_dev->print_status(false); (void)g_dev->print_debug(); printf("\n\n\n[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); @@ -3030,7 +3137,7 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { printf("[px4io] loaded\n"); - g_dev->print_status(); + g_dev->print_status(true); exit(0); } diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp index 19776c40a..c57ddf65b 100644..100755 --- a/src/drivers/px4io/px4io_i2c.cpp +++ b/src/drivers/px4io/px4io_i2c.cpp @@ -79,7 +79,7 @@ device::Device } PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) : - I2C("PX4IO_i2c", nullptr, bus, address, 320000) + I2C("PX4IO_i2c", nullptr, bus, address, 400000) { _retries = 3; } diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 43318ca84..c39494fb0 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt() if (_rx_dma_status == _dma_status_waiting) { /* verify that the received packet is complete */ - unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index dd8abbac5..d134c0246 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -39,6 +39,7 @@ #include <nuttx/config.h> #include <sys/types.h> +#include <stdlib.h> #include <stdint.h> #include <stdbool.h> #include <assert.h> @@ -201,9 +202,14 @@ PX4IO_Uploader::upload(const char *filenames[]) continue; } - if (bl_rev <= 2) + if (bl_rev <= 2) { ret = verify_rev2(fw_size); - else if(bl_rev == 3) { + } else if(bl_rev == 3) { + ret = verify_rev3(fw_size); + } else { + /* verify rev 4 and higher still uses the same approach and + * every version *needs* to be verified. + */ ret = verify_rev3(fw_size); } @@ -235,9 +241,9 @@ PX4IO_Uploader::upload(const char *filenames[]) close(_io_fd); _io_fd = -1; - // sleep for enough time for the IO chip to boot. This makes - // forceupdate more reliably startup IO again after update - up_udelay(100*1000); + // sleep for enough time for the IO chip to boot. This makes + // forceupdate more reliably startup IO again after update + up_udelay(100*1000); return ret; } @@ -408,11 +414,17 @@ static int read_with_retry(int fd, void *buf, size_t n) int PX4IO_Uploader::program(size_t fw_size) { - uint8_t file_buf[PROG_MULTI_MAX]; + uint8_t *file_buf; ssize_t count; int ret; size_t sent = 0; + file_buf = (uint8_t *)malloc(PROG_MULTI_MAX); + if (!file_buf) { + log("Can't allocate program buffer"); + return -ENOMEM; + } + log("programming %u bytes...", (unsigned)fw_size); ret = lseek(_fw_fd, 0, SEEK_SET); @@ -420,8 +432,8 @@ PX4IO_Uploader::program(size_t fw_size) while (sent < fw_size) { /* get more bytes to program */ size_t n = fw_size - sent; - if (n > sizeof(file_buf)) { - n = sizeof(file_buf); + if (n > PROG_MULTI_MAX) { + n = PROG_MULTI_MAX; } count = read_with_retry(_fw_fd, file_buf, n); @@ -433,8 +445,10 @@ PX4IO_Uploader::program(size_t fw_size) (int)errno); } - if (count == 0) + if (count == 0) { + free(file_buf); return OK; + } sent += count; @@ -450,9 +464,12 @@ PX4IO_Uploader::program(size_t fw_size) ret = get_sync(1000); - if (ret != OK) + if (ret != OK) { + free(file_buf); return ret; + } } + free(file_buf); return OK; } |