From ded8cc6e14c7ec42d2a0e08b83c1510f213bf55d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:56:39 +0100 Subject: Add IOCTLs to disable lockdown of an output port --- src/drivers/drv_pwm_output.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) (limited to 'src/drivers') diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 51f916f37..53065f8eb 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -189,6 +189,20 @@ ORB_DECLARE(output_pwm); /** get the maximum PWM value the output will send */ #define PWM_SERVO_GET_MAX_PWM _IOC(_PWM_SERVO_BASE, 19) +/** set the lockdown override flag to enable outputs in HIL */ +#define PWM_SERVO_SET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 21) + +/** get the lockdown override flag to enable outputs in HIL */ +#define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) + +/* + * + * + * WARNING WARNING WARNING! DO NOT EXCEED 31 IN IOC INDICES HERE! + * + * + */ + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) -- cgit v1.2.3 From 978cfdccce9148ebb2282646b609d5eb7c59f487 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Jan 2014 10:57:09 +0100 Subject: Teach IO driver how to disable lockdown mode --- src/drivers/px4io/px4io.cpp | 99 ++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 89 insertions(+), 10 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ca35d2f2..658bcd8d8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -273,6 +273,7 @@ private: actuator_outputs_s _outputs; ///ioctl(1, mode); /* XXX magic numbers */ - delete interface; + if (interface) { + result = interface->ioctl(1, mode); /* XXX magic numbers */ + delete interface; + } else { + errx(1, "interface not loaded, exiting"); + } errx(0, "test returned %d", result); } +void +lockdown(int argc, char *argv[]) +{ + if (g_dev != nullptr) { + + if (argc > 2 && !strcmp(argv[2], "disable")) { + + warnx("WARNING: ACTUATORS WILL BE LIVE IN HIL! PROCEED?"); + warnx("Press 'y' to enable, any other key to abort."); + + /* check if user wants to abort */ + char c; + + struct pollfd fds; + int ret; + hrt_abstime start = hrt_absolute_time(); + const unsigned long timeout = 5000000; + + while (hrt_elapsed_time(&start) < timeout) { + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + + if (c != 'y') { + exit(0); + } else if (c == 'y') { + break; + } + } + + usleep(10000); + } + + if (hrt_elapsed_time(&start) > timeout) + errx(1, "TIMEOUT! ABORTED WITHOUT CHANGES."); + + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 1); + + warnx("WARNING: ACTUATORS ARE NOW LIVE IN HIL!"); + } else { + (void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0); + warnx("ACTUATORS ARE NOW SAFE IN HIL."); + } + + } else { + errx(1, "driver not loaded, exiting"); + } + exit(0); +} + } /* namespace */ int @@ -2883,6 +2959,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); + if (!strcmp(argv[1], "lockdown")) + lockdown(argc, argv); + out: errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); } -- cgit v1.2.3 From f2f94f0f176b9a2d818a0849eae18f018d7fb5a9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:03:21 +0100 Subject: IO driver: Variable name and comment cleanup, no binary / functionality changes. --- src/drivers/px4io/px4io.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df847a64d..e83fa68d5 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -444,7 +444,7 @@ private: * @param vservo vservo register * @param vrssi vrssi register */ - void io_handle_vservo(uint16_t vbatt, uint16_t ibatt); + void io_handle_vservo(uint16_t vservo, uint16_t vrssi); }; @@ -1357,7 +1357,10 @@ PX4IO::io_get_status() uint16_t regs[6]; int ret; - /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ + /* get + * STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT, + * STATUS_VSERVO, STATUS_VRSSI, STATUS_PRSSI + * in that order */ ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, ®s[0], sizeof(regs) / sizeof(regs[0])); if (ret != OK) -- cgit v1.2.3 From bafcbd99a695a3c4d478fb58e1d53940f331392f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 25 Jan 2014 23:04:16 +0100 Subject: Stop setting RSSI by cross-reading servo status. --- src/drivers/px4io/px4io.cpp | 7 ------- 1 file changed, 7 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e83fa68d5..4bdf530a7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1457,13 +1457,6 @@ PX4IO::io_publish_raw_rc() rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN; } - /* set RSSI */ - - if (rc_val.input_source != RC_INPUT_SOURCE_PX4IO_SBUS) { - // XXX the correct scaling needs to be validated here - rc_val.rssi = (_servorail_status.rssi_v / 3.3f) * UINT8_MAX; - } - /* lazily advertise on first publication */ if (_to_input_rc == 0) { _to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_val); -- cgit v1.2.3 From eee2508644ea1ca3b267ed89db6d0deb8fe0d3e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 00:14:19 +0100 Subject: Add additional flags to RC topic, not used yet. --- src/drivers/drv_rc_input.h | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) (limited to 'src/drivers') diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 6b87141e9..0afe2f16f 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -92,6 +92,34 @@ struct rc_input_values { /** receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 255: full reception */ int32_t rssi; + /** + * explicit failsafe flag: true on TX failure or TX out of range , false otherwise. + * Only the true state is reliable, as there are some (PPM) receivers on the market going + * into failsafe without telling us explicitly. + * */ + bool rc_failsafe; + + /** + * RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. + * True usally means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. + * Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. + * */ + bool rc_lost; + + /** + * Number of lost RC frames. + * Note: intended purpose: observe the radio link quality if RSSI is not available + * This value must not be used to trigger any failsafe-alike funtionality. + * */ + uint16_t rc_lost_frame_count; + + /** + * Number of total RC frames. + * Note: intended purpose: observe the radio link quality if RSSI is not available + * This value must not be used to trigger any failsafe-alike funtionality. + * */ + uint16_t rc_total_frame_count; + /** Input source */ enum RC_INPUT_SOURCE input_source; -- cgit v1.2.3 From 57d38bc8cec1362308f632e74e99485f82a35501 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 00:17:26 +0100 Subject: Clean up RC related metadata, put everything into the RC data page. This ensures atomic reads, makes the reads more efficient and allows for some headroom for more RC flags. The IO driver side is updated as well, however, these flags are not published yet. --- src/drivers/px4io/px4io.cpp | 10 ++++--- src/modules/px4iofirmware/controls.c | 49 ++++++++++++++++++++++++++++------- src/modules/px4iofirmware/protocol.h | 14 +++++++--- src/modules/px4iofirmware/px4io.h | 3 ++- src/modules/px4iofirmware/registers.c | 8 ++++-- src/modules/px4iofirmware/sbus.c | 21 ++++++++------- 6 files changed, 76 insertions(+), 29 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 4bdf530a7..382acb6d0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1763,6 +1763,7 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); + uint16_t io_status_flags = flags; printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), @@ -1770,8 +1771,6 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PWM_PASSTHROUGH" : ""), @@ -1824,6 +1823,11 @@ PX4IO::print_status() printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); + printf("status 0x%04x%s", flags, + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "") + ); for (unsigned i = 0; i < raw_inputs; i++) printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); @@ -1831,7 +1835,7 @@ PX4IO::print_status() printf("\n"); if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { - int frame_len = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_RC_DATA); + int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); if ((frame_len - raw_inputs * 2000 - 3000) < 0) { diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index ed37cc43d..1cac977d7 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -97,28 +97,57 @@ controls_tick() { /* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */ uint16_t rssi = 0; +#ifdef ADC_RSSI + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + /* use 1:1 scaling on 3.3V ADC input */ + unsigned mV = counts * 3300 / 4096; + + /* scale to 0..253 */ + rssi = mV / 13; + } +#endif + perf_begin(c_gather_dsm); uint16_t temp_count = r_raw_rc_count; bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); if (dsm_updated) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; r_raw_rc_count = temp_count & 0x7fff; if (temp_count & 0x8000) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11; else - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11; - rssi = 255; } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &rssi, PX4IO_RC_INPUT_CHANNELS); bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS); + bool sbus_failsafe, sbus_frame_drop; + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS); + if (sbus_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + + rssi = 255; + + if (sbus_frame_drop) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP; + rssi = 100; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP); + } + + if (sbus_failsafe) { + r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE; + rssi = 0; + } else { + r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE); + } + } /* switch S.Bus output pin as needed */ @@ -136,12 +165,9 @@ controls_tick() { * disable the PPM decoder completely if we have S.bus signal. */ perf_begin(c_gather_ppm); - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_status[PX4IO_P_STATUS_RC_DATA]); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]); if (ppm_updated) { - /* XXX sample RSSI properly here */ - rssi = 255; - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; } perf_end(c_gather_ppm); @@ -150,6 +176,9 @@ controls_tick() { if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS; + /* store RSSI */ + r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi; + /* * In some cases we may have received a frame, but input has still * been lost. @@ -247,7 +276,7 @@ controls_tick() { * This might happen if a protocol-based receiver returns an update * that contains no channels that we have mapped. */ - if (assigned_channels == 0 || rssi == 0) { + if (assigned_channels == 0 || (r_raw_rc_flags & (PX4IO_P_RAW_RC_FLAGS_FAILSAFE))) { rc_input_lost = true; } else { /* set RC OK flag */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index e5bef6eb3..738dc7d6e 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -111,7 +111,6 @@ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ -#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 13) /* DSM input is 11 bit data */ #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) /* [1] VBatt is very close to regulator dropout */ @@ -128,8 +127,6 @@ #define PX4IO_P_STATUS_VSERVO 6 /* [2] servo rail voltage in mV */ #define PX4IO_P_STATUS_VRSSI 7 /* [2] RSSI voltage */ #define PX4IO_P_STATUS_PRSSI 8 /* [2] RSSI PWM value */ -#define PX4IO_P_STATUS_NRSSI 9 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ -#define PX4IO_P_STATUS_RC_DATA 10 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ /* array of post-mix actuator outputs, -10000..10000 */ #define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */ @@ -140,7 +137,16 @@ /* 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 */ +#define PX4IO_P_RAW_RC_FLAGS 1 /* RC detail status flags */ +#define PX4IO_P_RAW_RC_FLAGS_FRAME_DROP (1 << 0) /* single frame drop */ +#define PX4IO_P_RAW_RC_FLAGS_FAILSAFE (1 << 1) /* receiver is in failsafe mode */ +#define PX4IO_P_RAW_RC_FLAGS_RC_DSM11 (1 << 2) /* DSM decoding is 11 bit mode */ + +#define PX4IO_P_RAW_RC_NRSSI 2 /* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */ +#define PX4IO_P_RAW_RC_DATA 3 /* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */ +#define PX4IO_P_RAW_FRAME_COUNT 4 /* Number of total received frames (wrapping counter) */ +#define PX4IO_P_RAW_LOST_FRAME_COUNT 5 /* Number of total dropped frames (wrapping counter) */ +#define PX4IO_P_RAW_RC_BASE 6 /* CONFIG_RC_INPUT_COUNT channels from here */ /* array of scaled RC input values, -10000..10000 */ #define PX4IO_PAGE_RC_INPUT 5 diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index ee9d5b0c4..a2e1ed6cd 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -96,6 +96,7 @@ extern uint16_t r_page_servo_disarmed[]; /* PX4IO_PAGE_DISARMED_PWM */ #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_raw_rc_flags r_page_raw_rc_input[PX4IO_P_RAW_RC_FLAGS] #define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID] #define r_rc_values (&r_page_rc_input[PX4IO_P_RC_BASE]) @@ -215,7 +216,7 @@ extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); -extern bool sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 2c437d2c0..477efa082 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -90,8 +90,6 @@ uint16_t r_page_status[] = { [PX4IO_P_STATUS_VSERVO] = 0, [PX4IO_P_STATUS_VRSSI] = 0, [PX4IO_P_STATUS_PRSSI] = 0, - [PX4IO_P_STATUS_NRSSI] = 0, - [PX4IO_P_STATUS_RC_DATA] = 0 }; /** @@ -116,6 +114,12 @@ uint16_t r_page_servos[PX4IO_SERVO_COUNT]; uint16_t r_page_raw_rc_input[] = { [PX4IO_P_RAW_RC_COUNT] = 0, + [PX4IO_P_RAW_RC_FLAGS] = 0, + [PX4IO_P_RAW_RC_NRSSI] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, + [PX4IO_P_RAW_FRAME_COUNT] = 0, + [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, + [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 495447740..32ab2a698 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -87,7 +87,7 @@ static unsigned partial_frame_count; unsigned sbus_frame_drops; -static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels); +static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); int sbus_init(const char *device) @@ -118,7 +118,7 @@ sbus_init(const char *device) } bool -sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_channels) +sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { ssize_t ret; hrt_abstime now; @@ -175,7 +175,7 @@ sbus_input(uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_ * decode it. */ partial_frame_count = 0; - return sbus_decode(now, values, num_values, rssi, max_channels); + return sbus_decode(now, values, num_values, sbus_failsafe, sbus_frame_drop, max_channels); } /* @@ -215,7 +215,7 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = { }; static bool -sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint16_t *rssi, uint16_t max_values) +sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_values) { /* check frame boundary markers to avoid out-of-sync cases */ if ((frame[0] != 0x0f)) { @@ -289,20 +289,23 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values, uint /* decode and handle failsafe and frame-lost flags */ if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FAILSAFE_BIT)) { /* failsafe */ /* report that we failed to read anything valid off the receiver */ - *rssi = 0; + *sbus_failsafe = true; + *sbus_frame_drop = true; return false; } else if (frame[SBUS_FLAGS_BYTE] & (1 << SBUS_FRAMELOST_BIT)) { /* a frame was lost */ - /* set a special warning flag or try to calculate some kind of RSSI information - to be implemented + /* set a special warning flag * * Attention! This flag indicates a skipped frame only, not a total link loss! Handling this * condition as fail-safe greatly reduces the reliability and range of the radio link, * e.g. by prematurely issueing return-to-launch!!! */ - *rssi = 100; // XXX magic number indicating bad signal, but not a signal loss (yet) + *sbus_failsafe = false; + *sbus_frame_drop = true; + } else { + *sbus_failsafe = false; + *sbus_frame_drop = false; } - *rssi = 255; - return true; } -- cgit v1.2.3 From 731ab465b3d7d40ffb5ce3ca3d14660c6fee1ae6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:22:54 +0100 Subject: Add support in the IO driver to control the S.Bus / RSSI port. --- src/drivers/drv_rc_input.h | 8 +++- src/drivers/drv_sbus.h | 58 +++++++++++++++++++++++++ src/drivers/px4io/px4io.cpp | 103 ++++++++++++++++++++++++++++++++++++++++++-- 3 files changed, 163 insertions(+), 6 deletions(-) create mode 100644 src/drivers/drv_sbus.h (limited to 'src/drivers') diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 0afe2f16f..715df7e01 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 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 @@ -135,8 +135,12 @@ 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) +/** Enable RSSI input via ADC */ +#define RC_INPUT_ENABLE_RSSI_ANALOG _IOC(_RC_INPUT_BASE, 1) + +/** Enable RSSI input via PWM signal */ +#define RC_INPUT_ENABLE_RSSI_PWM _IOC(_RC_INPUT_BASE, 2) #endif /* _DRV_RC_INPUT_H */ diff --git a/src/drivers/drv_sbus.h b/src/drivers/drv_sbus.h new file mode 100644 index 000000000..927c904ec --- /dev/null +++ b/src/drivers/drv_sbus.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 drv_sbus.h + * + * Futaba S.BUS / S.BUS 2 compatible interface. + */ + +#ifndef _DRV_SBUS_H +#define _DRV_SBUS_H + +#include +#include + +#include "drv_orb_dev.h" + +/** + * Path for the default S.BUS device + */ +#define SBUS_DEVICE_PATH "/dev/sbus" + +#define _SBUS_BASE 0x2c00 + +/** Enable S.BUS version 1 / 2 output (0 to disable) */ +#define SBUS_SET_PROTO_VERSION _IOC(_SBUS_BASE, 0) + +#endif /* _DRV_SBUS_H */ diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 382acb6d0..0fb7b7d24 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -1824,9 +1825,11 @@ PX4IO::print_status() uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("status 0x%04x%s", flags, + printf("status 0x%04x%s%s%s%s", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), - (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : "") + (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") ); for (unsigned i = 0; i < raw_inputs; i++) @@ -1861,7 +1864,13 @@ PX4IO::print_status() printf("\n"); /* setup and state */ - printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); + uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); + printf("features 0x%04x%s%s\n", features, + ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), + ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), + ((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "") + ); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s\n", arming, @@ -2283,6 +2292,38 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) break; + case RC_INPUT_ENABLE_RSSI_ANALOG: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_ADC_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_ADC_RSSI, 0); + } + + break; + + case RC_INPUT_ENABLE_RSSI_PWM: + + if (arg) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_PWM_RSSI); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, PX4IO_P_SETUP_FEATURES_PWM_RSSI, 0); + } + + break; + + case SBUS_SET_PROTO_VERSION: + + if (arg == 1) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS1_OUT); + } else if (arg == 2) { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, 0, PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } else { + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES, (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT), 0); + } + + break; + default: /* not a recognized value */ ret = -ENOTTY; @@ -2932,6 +2973,60 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "bind")) bind(argc, argv); + if (!strcmp(argv[1], "sbus1_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 1); + + if (ret != 0) { + errx(ret, "S.BUS v1 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "sbus2_out")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, SBUS_SET_PROTO_VERSION, 2); + + if (ret != 0) { + errx(ret, "S.BUS v2 failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_analog")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_ANALOG, 1); + + if (ret != 0) { + errx(ret, "RSSI analog failed"); + } + + exit(0); + } + + if (!strcmp(argv[1], "rssi_pwm")) { + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + int ret = g_dev->ioctl(nullptr, RC_INPUT_ENABLE_RSSI_PWM, 1); + + if (ret != 0) { + errx(ret, "RSSI PWM failed"); + } + + exit(0); + } + out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'bind', 'checkcrc', 'forceupdate' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug ',\n" + "'recovery', 'limit ', 'current', 'bind', 'checkcrc',\n" + "'forceupdate', 'update', 'sbus1_out', 'sbus2_out', 'rssi_analog' or 'rssi_pwm'"); } -- cgit v1.2.3 From 9cdbbab855d463bffb39d8dd55888fc1e0423818 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 14:52:46 +0100 Subject: Differentiate between publication and signal receive timestamp, correctly set the rc_lost flag in the frame. Ready for prime-time testing. --- src/drivers/drv_rc_input.h | 18 ++++++++++++++++-- src/drivers/px4fmu/fmu.cpp | 12 ++++++++++-- src/drivers/px4io/px4io.cpp | 31 +++++++++++++++++++++++++------ src/modules/mavlink/orb_listener.c | 2 +- src/modules/sensors/sensors.cpp | 9 ++++++--- src/systemcmds/tests/test_rc.c | 2 +- 6 files changed, 59 insertions(+), 15 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 715df7e01..20763e265 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -62,6 +62,11 @@ */ #define RC_INPUT_MAX_CHANNELS 18 +/** + * Maximum RSSI value + */ +#define RC_INPUT_RSSI_MAX 255 + /** * Input signal type, value is a control position from zero to 100 * percent. @@ -83,8 +88,11 @@ enum RC_INPUT_SOURCE { * on the board involved. */ struct rc_input_values { - /** decoding time */ - uint64_t timestamp; + /** publication time */ + uint64_t timestamp_publication; + + /** last valid reception time */ + uint64_t timestamp_last_signal; /** number of channels actually being seen */ uint32_t channel_count; @@ -120,6 +128,12 @@ struct rc_input_values { * */ uint16_t rc_total_frame_count; + /** + * Length of a single PPM frame. + * Zero for non-PPM systems + */ + uint16_t rc_ppm_frame_length; + /** Input source */ enum RC_INPUT_SOURCE input_source; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index c067d363b..0fbd84924 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -626,7 +626,7 @@ PX4FMU::task_main() #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data - if (ppm_last_valid_decode != rc_in.timestamp) { + if (ppm_last_valid_decode != rc_in.timestamp_last_signal) { // we have a new PPM frame. Publish it. rc_in.channel_count = ppm_decoded_channels; @@ -638,7 +638,15 @@ PX4FMU::task_main() rc_in.values[i] = ppm_buffer[i]; } - rc_in.timestamp = ppm_last_valid_decode; + rc_in.timestamp_publication = ppm_last_valid_decode; + rc_in.timestamp_last_signal = ppm_last_valid_decode; + + rc_in.rc_ppm_frame_length = ppm_frame_length; + rc_in.rssi = RC_INPUT_RSSI_MAX; + rc_in.rc_failsafe = false; + rc_in.rc_lost = false; + rc_in.rc_lost_frame_count = 0; + rc_in.rc_total_frame_count = 0; /* lazily advertise on first publication */ if (to_input_rc == 0) { diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0fb7b7d24..5601230a4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -239,6 +239,7 @@ private: unsigned _update_interval; ///< Subscription interval limiting send rate bool _rc_handling_disabled; ///< If set, IO does not evaluate, but only forward the RC values unsigned _rc_chan_count; ///< Internal copy of the last seen number of RC channels + uint64_t _rc_last_valid; ///< last valid timestamp volatile int _task; ///< worker task id volatile bool _task_should_exit; ///< worker terminate flag @@ -468,6 +469,7 @@ PX4IO::PX4IO(device::Device *interface) : _update_interval(0), _rc_handling_disabled(false), _rc_chan_count(0), + _rc_last_valid(0), _task(-1), _task_should_exit(false), _mavlink_fd(-1), @@ -1398,7 +1400,8 @@ 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 = hrt_absolute_time(); + 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) @@ -1408,13 +1411,25 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) * Get the channel count any any extra channels. This is no more expensive than reading the * channel count once. */ - channel_count = regs[0]; + channel_count = regs[PX4IO_P_RAW_RC_COUNT]; if (channel_count != _rc_chan_count) perf_count(_perf_chan_count); _rc_chan_count = channel_count; + 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_frame_count = regs[PX4IO_P_RAW_LOST_FRAME_COUNT]; + input_rc.rc_total_frame_count = regs[PX4IO_P_RAW_FRAME_COUNT]; + + /* rc_lost has to be set before the call to this function */ + if (!input_rc.rc_lost && !input_rc.rc_failsafe) + _rc_last_valid = input_rc.timestamp_publication; + + input_rc.timestamp_last_signal = _rc_last_valid; + 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); @@ -1431,13 +1446,12 @@ PX4IO::io_get_raw_rc_input(rc_input_values &input_rc) 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(); + + /* set the RC status flag ORDER MATTERS! */ + rc_val.rc_lost = !(_status & PX4IO_P_STATUS_FLAGS_RC_OK); int ret = io_get_raw_rc_input(rc_val); @@ -1456,6 +1470,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)) + return OK; } /* lazily advertise on first publication */ diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 92b1b45be..41c754405 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -355,7 +355,7 @@ l_input_rc(const struct listener *l) for (unsigned i = 0; (i * port_width) < (rc_raw.channel_count + port_width); i++) { /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc_raw.timestamp / 1000, + rc_raw.timestamp_publication / 1000, i, (rc_raw.channel_count > (i * port_width) + 0) ? rc_raw.values[(i * port_width) + 0] : UINT16_MAX, (rc_raw.channel_count > (i * port_width) + 1) ? rc_raw.values[(i * port_width) + 1] : UINT16_MAX, diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 23f20b0cb..df6cbb7b2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1276,6 +1276,9 @@ Sensors::rc_poll() orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + if (rc_input.rc_lost) + return; + struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1320,7 +1323,7 @@ Sensors::rc_poll() channel_limit = _rc_max_chan_count; /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp; + _rc_last_valid = rc_input.timestamp_last_signal; /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1369,9 +1372,9 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp; + _rc.timestamp = rc_input.timestamp_last_signal; - manual_control.timestamp = rc_input.timestamp; + manual_control.timestamp = rc_input.timestamp_last_signal; /* roll input - rolling right is stick-wise and rotation-wise positive */ manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 6a602ecfc..57c0e7f4c 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -121,7 +121,7 @@ int test_rc(int argc, char *argv[]) return ERROR; } - if (hrt_absolute_time() - rc_input.timestamp > 100000) { + if (hrt_absolute_time() - rc_input.timestamp_last_signal > 100000) { warnx("TIMEOUT, less than 10 Hz updates"); (void)close(_rc_sub); return ERROR; -- cgit v1.2.3 From b67f7b2c7cb4d176ff8b3c5b7ee5e839845eef94 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:12:12 +0100 Subject: Fix printing of IO status --- src/drivers/px4io/px4io.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5601230a4..e24236de7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1784,7 +1784,7 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); uint16_t io_status_flags = flags; - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), @@ -1843,6 +1843,12 @@ PX4IO::print_status() printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); printf("%d raw R/C inputs", raw_inputs); + + for (unsigned i = 0; i < raw_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + + printf("\n"); + flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); printf("status 0x%04x%s%s%s%s", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), @@ -1851,11 +1857,6 @@ PX4IO::print_status() ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") ); - for (unsigned i = 0; i < raw_inputs; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); - - printf("\n"); - if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -1884,7 +1885,7 @@ PX4IO::print_status() /* setup and state */ uint16_t features = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES); - printf("features 0x%04x%s%s\n", features, + printf("features 0x%04x%s%s%s%s\n", features, ((features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) ? " S.BUS1_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) ? " S.BUS2_OUT" : ""), ((features & PX4IO_P_SETUP_FEATURES_PWM_RSSI) ? " RSSI_PWM" : ""), -- cgit v1.2.3 From 2a30c574ce569c876dd2b95919a0d86c7c1b1023 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 26 Jan 2014 15:50:19 +0100 Subject: IO status printing improvements / fixes --- src/drivers/px4io/px4io.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index e24236de7..9595518ec 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1850,14 +1850,15 @@ PX4IO::print_status() printf("\n"); flags = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_FLAGS); - printf("status 0x%04x%s%s%s%s", flags, + printf("R/C flags: 0x%04x%s%s%s%s%s\n", flags, (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11))) ? " DSM10" : ""), (((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_RAW_RC_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_RAW_RC_FLAGS_FRAME_DROP) ? " FRAME_DROP" : ""), - ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : "") + ((flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE) ? " FAILSAFE" : ""), + ((flags & PX4IO_P_RAW_RC_FLAGS_MAPPING_OK) ? " MAPPING_OK" : "") ); - if ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { + if ((io_status_flags & PX4IO_P_STATUS_FLAGS_RC_PPM)) { int frame_len = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_DATA); printf("RC data (PPM frame len) %u us\n", frame_len); @@ -2717,7 +2718,7 @@ monitor(void) /* clear screen */ printf("\033[2J"); - unsigned cancels = 3; + unsigned cancels = 2; for (;;) { pollfd fds[1]; -- cgit v1.2.3 From dfaa5a0c7c81f432d64cfffdaaf3d683c01f10b4 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 28 Jan 2014 09:57:47 +0800 Subject: Reduced stack from 2048 to 1024. Top reports stack usage at 812 under flight conditions. --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index df847a64d..dbe97a178 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -756,7 +756,7 @@ 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_create("px4io", SCHED_PRIORITY_ACTUATOR_OUTPUTS, 1024, (main_t)&PX4IO::task_main_trampoline, nullptr); if (_task < 0) { debug("task start failed: %d", errno); -- cgit v1.2.3 From 1d70a65f40895b0c9e1ee5c65970efa1eae841e8 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Wed, 29 Jan 2014 17:52:22 +0800 Subject: Stack size reduced to 1280. Max stack size reported as 1020 with UBX GPS with 3d lock. MTK not tested. --- src/drivers/gps/gps.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/drivers') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6b72d560f..f2faf711b 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -209,7 +209,7 @@ GPS::init() goto out; /* start the GPS driver worker task */ - _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); + _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 1280, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); -- cgit v1.2.3