From d6c108d870034a8dfc328487dc3477738937894d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 17 Feb 2013 16:20:23 +1100 Subject: [PATCH 01/13] px4fmu: added publication of input_rc ORB values this allows for PPM input with no IO board --- apps/drivers/px4fmu/fmu.cpp | 34 ++++++++++++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 430d18c6d5..ed3635fc95 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -64,12 +64,14 @@ #include #include #include +#include #include #include #include #include +#include class PX4FMU : public device::CDev { @@ -338,6 +340,13 @@ PX4FMU::task_main() unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; + // rc input, published to ORB + struct rc_input_values rc_in; + orb_advert_t to_input_rc = 0; + + memset(&rc_in, 0, sizeof(rc_in)); + rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM; + log("starting"); /* loop until killed */ @@ -363,8 +372,9 @@ PX4FMU::task_main() _current_update_rate = _update_rate; } - /* sleep waiting for data, but no more than a second */ - int ret = ::poll(&fds[0], 2, 1000); + /* sleep waiting for data, stopping to check for PPM + * input at 100Hz */ + int ret = ::poll(&fds[0], 2, 10); /* this would be bad... */ if (ret < 0) { @@ -429,6 +439,26 @@ PX4FMU::task_main() /* update PWM servo armed status if armed and not locked down */ up_pwm_servo_arm(aa.armed && !aa.lockdown); } + + // see if we have new PPM input data + if (ppm_last_valid_decode != rc_in.timestamp) { + // we have a new PPM frame. Publish it. + rc_in.channel_count = ppm_decoded_channels; + if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) { + rc_in.channel_count = RC_INPUT_MAX_CHANNELS; + } + for (uint8_t i=0; i Date: Sun, 17 Feb 2013 16:43:45 +1100 Subject: [PATCH 02/13] px4fmu: add support for write() interface for PWM output this matches the PX4IO interface --- apps/drivers/px4fmu/fmu.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index ed3635fc95..c29fe0ba3d 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -85,6 +85,7 @@ public: ~PX4FMU(); virtual int ioctl(file *filp, int cmd, unsigned long arg); + virtual ssize_t write(file *filp, const char *buffer, size_t len); virtual int init(); @@ -651,6 +652,30 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) return ret; } +/* + this implements PWM output via a write() method, for compatibility + with px4io + */ +ssize_t +PX4FMU::write(file *filp, const char *buffer, size_t len) +{ + unsigned count = len / 2; + uint16_t values[4]; + + if (count > 4) { + // we only have 4 PWM outputs on the FMU + count = 4; + } + + // allow for misaligned values + memcpy(values, buffer, count*2); + + for (uint8_t i=0; i Date: Tue, 22 Jan 2013 07:48:30 +1100 Subject: [PATCH 03/13] appconfig: disable mathlib and associated examples on APM these are far too large (777 kbyte) and we can't fit them with the ArduCopter flight code --- nuttx/configs/px4fmu/nsh/appconfig | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index a904d90ac5..cb9ddee5f4 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -46,14 +46,18 @@ CONFIGURED_APPS += systemlib CONFIGURED_APPS += systemlib/mixer # Math library +ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += mathlib CONFIGURED_APPS += mathlib/CMSIS CONFIGURED_APPS += examples/math_demo +endif # Control library +ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += controllib CONFIGURED_APPS += examples/control_demo CONFIGURED_APPS += examples/kalman_demo +endif # System utility commands CONFIGURED_APPS += systemcmds/reboot From 1670b8afe13ba6c845800228d1c8829aa1bf31c9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Feb 2013 10:19:38 +1100 Subject: [PATCH 04/13] nshlib: added cmp command to nsh this is useful for startup scripts testing for auto-upgrade of add-on board firmware --- apps/nshlib/Kconfig | 4 ++ apps/nshlib/nsh.h | 3 ++ apps/nshlib/nsh_fscmds.c | 81 ++++++++++++++++++++++++++++++++++++++++ apps/nshlib/nsh_parse.c | 3 ++ 4 files changed, 91 insertions(+) diff --git a/apps/nshlib/Kconfig b/apps/nshlib/Kconfig index 92bc83cfd0..d7a7b8a991 100644 --- a/apps/nshlib/Kconfig +++ b/apps/nshlib/Kconfig @@ -55,6 +55,10 @@ config NSH_DISABLE_CP bool "Disable cp" default n +config NSH_DISABLE_CMP + bool "Disable cmp" + default n + config NSH_DISABLE_DD bool "Disable dd" default n diff --git a/apps/nshlib/nsh.h b/apps/nshlib/nsh.h index 23209dba52..83cf25aa7f 100644 --- a/apps/nshlib/nsh.h +++ b/apps/nshlib/nsh.h @@ -603,6 +603,9 @@ void nsh_usbtrace(void); # ifndef CONFIG_NSH_DISABLE_CP int cmd_cp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); # endif +# ifndef CONFIG_NSH_DISABLE_CMP + int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); +# endif # ifndef CONFIG_NSH_DISABLE_DD int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv); # endif diff --git a/apps/nshlib/nsh_fscmds.c b/apps/nshlib/nsh_fscmds.c index f47dca896f..83717e416c 100644 --- a/apps/nshlib/nsh_fscmds.c +++ b/apps/nshlib/nsh_fscmds.c @@ -1232,3 +1232,84 @@ int cmd_sh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) } #endif #endif + + +#if CONFIG_NFILE_DESCRIPTORS > 0 +#ifndef CONFIG_NSH_DISABLE_CMP +int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv) +{ + char *path1 = NULL; + char *path2 = NULL; + int fd1 = -1, fd2 = -1; + int ret = ERROR; + unsigned total_read = 0; + + /* Get the full path to the two files */ + + path1 = nsh_getfullpath(vtbl, argv[1]); + if (!path1) + { + goto errout; + } + + path2 = nsh_getfullpath(vtbl, argv[2]); + if (!path2) + { + goto errout; + } + + /* Open the files for reading */ + fd1 = open(path1, O_RDONLY); + if (fd1 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO); + goto errout; + } + + fd2 = open(path2, O_RDONLY); + if (fd2 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO); + goto errout; + } + + for (;;) + { + char buf1[128]; + char buf2[128]; + + int nbytesread1 = read(fd1, buf1, sizeof(buf1)); + int nbytesread2 = read(fd2, buf2, sizeof(buf2)); + + if (nbytesread1 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO); + goto errout; + } + + if (nbytesread2 < 0) + { + nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO); + goto errout; + } + + total_read += nbytesread1>nbytesread2?nbytesread2:nbytesread1; + + if (nbytesread1 != nbytesread2 || memcmp(buf1, buf2, nbytesread1) != 0) + { + nsh_output(vtbl, "files differ: byte %u\n", total_read); + goto errout; + } + + if (nbytesread1 < sizeof(buf1)) break; + } + + ret = OK; + +errout: + if (fd1 != -1) close(fd1); + if (fd2 != -1) close(fd2); + return ret; +} +#endif +#endif diff --git a/apps/nshlib/nsh_parse.c b/apps/nshlib/nsh_parse.c index f679d9b320..4d8f04b23f 100644 --- a/apps/nshlib/nsh_parse.c +++ b/apps/nshlib/nsh_parse.c @@ -175,6 +175,9 @@ static const struct cmdmap_s g_cmdmap[] = # ifndef CONFIG_NSH_DISABLE_CP { "cp", cmd_cp, 3, 3, " " }, # endif +# ifndef CONFIG_NSH_DISABLE_CMP + { "cmp", cmd_cmp, 3, 3, " " }, +# endif #endif #if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE) From 317515fb6a3d6e469681b53316d69c7669efdaf0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Feb 2013 14:16:09 +1100 Subject: [PATCH 05/13] px4io: added INAIR_RESTART enable/disable flags the autopilot code needs to know that in-air restart may happen, so it should be something that is enabled, rather than on by default. --- apps/drivers/drv_pwm_output.h | 6 ++++++ apps/drivers/px4io/px4io.cpp | 18 +++++++++++++++++- apps/px4io/protocol.h | 1 + apps/px4io/registers.c | 3 ++- 4 files changed, 26 insertions(+), 2 deletions(-) diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h index 64bb540b42..f3f753ebe6 100644 --- a/apps/drivers/drv_pwm_output.h +++ b/apps/drivers/drv_pwm_output.h @@ -109,6 +109,12 @@ ORB_DECLARE(output_pwm); /** set debug level for servo IO */ #define PWM_SERVO_SET_DEBUG _IOC(_PWM_SERVO_BASE, 4) +/** enable in-air restart */ +#define PWM_SERVO_INAIR_RESTART_ENABLE _IOC(_PWM_SERVO_BASE, 5) + +/** disable in-air restart */ +#define PWM_SERVO_INAIR_RESTART_DISABLE _IOC(_PWM_SERVO_BASE, 6) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index b2e0c6ee45..adb894371f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -367,7 +367,12 @@ PX4IO::init() if (ret != OK) return ret; - if (reg & PX4IO_P_SETUP_ARMING_ARM_OK) { + /* + * in-air restart is only tried if the IO board reports it is + * already armed, and has been configured for in-air restart + */ + if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && + (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 @@ -450,6 +455,7 @@ PX4IO::init() /* 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_INAIR_RESTART_OK | PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); @@ -1164,6 +1170,16 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_ARM_OK, 0); break; + case PWM_SERVO_INAIR_RESTART_ENABLE: + /* set the 'in-air restart' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK); + break; + + case PWM_SERVO_INAIR_RESTART_DISABLE: + /* unset the 'in-air restart' bit */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK, 0); + break; + case PWM_SERVO_SET_UPDATE_RATE: /* set the requested rate */ if ((arg >= 50) && (arg <= 400)) { diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index a957a9e79a..0ee6d2c4b4 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -142,6 +142,7 @@ #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_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #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 */ diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 23ad4aa884..ec1980aaaf 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -142,7 +142,8 @@ volatile uint16_t r_page_setup[] = #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) + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ + PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) From a68300941f4492553d865d63b20683adb292c2fb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Feb 2013 14:16:29 +1100 Subject: [PATCH 06/13] px4fmu: enable BINFS needed for APM startup --- nuttx/configs/px4fmu/nsh/defconfig | 1 + 1 file changed, 1 insertion(+) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 3fd27db113..5d6900ab35 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -779,6 +779,7 @@ CONFIG_NXFFS_MAXNAMLEN=32 CONFIG_NXFFS_TAILTHRESHOLD=2048 CONFIG_NXFFS_PREALLOCATED=y CONFIG_FS_ROMFS=y +CONFIG_FS_BINFS=y # # SPI-based MMC/SD driver From 3c6d6f0ef1cf67ebcb1c28f142da73f8c6fcf91d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 18 Feb 2013 14:17:06 +1100 Subject: [PATCH 07/13] px4fmu: disable a bunch of code when built for APM this leaves us enough flash to fit APM --- nuttx/configs/px4fmu/nsh/appconfig | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index cb9ddee5f4..517399a051 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -87,6 +87,8 @@ CONFIGURED_APPS += mavlink_onboard CONFIGURED_APPS += commander CONFIGURED_APPS += sdlog CONFIGURED_APPS += sensors + +ifneq ($(CONFIG_APM),y) CONFIGURED_APPS += ardrone_interface CONFIGURED_APPS += multirotor_att_control CONFIGURED_APPS += multirotor_pos_control @@ -96,6 +98,7 @@ CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += hott_telemetry +endif # Hacking tools #CONFIGURED_APPS += system/i2c From 104d5aa3654545b354f25750d3980181da8f6a0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Feb 2013 16:45:59 +0100 Subject: [PATCH 08/13] More sensible error handling in calibration --- apps/commander/commander.c | 28 +++++++++++++++++----------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 3a6fecf746..333dcca3e7 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -421,7 +421,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) // mavlink_log_info(mavlink_fd, buf); // } - if (poll(fds, 1, 1000)) { + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); x[calibration_counter] = mag.x; @@ -453,9 +455,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; - } else { + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled"); + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); break; } } @@ -545,7 +547,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* third beep by cal end routine */ } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); } /* disable calibration mode */ @@ -590,14 +592,16 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) /* wait blocking for new data */ struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - if (poll(fds, 1, 1000)) { + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); gyro_offset[0] += raw.gyro_rad_s[0]; gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; - } else { + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); return; @@ -698,14 +702,16 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) /* wait blocking for new data */ struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - if (poll(fds, 1, 1000)) { + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); accel_offset[0] += raw.accelerometer_m_s2[0]; accel_offset[1] += raw.accelerometer_m_s2[1]; accel_offset[2] += raw.accelerometer_m_s2[2]; calibration_counter++; - } else { + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "acceleration calibration aborted"); return; @@ -1225,7 +1231,6 @@ int commander_main(int argc, char *argv[]) 4000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; exit(0); } @@ -1311,7 +1316,8 @@ int commander_thread_main(int argc, char *argv[]) memset(&home, 0, sizeof(home)); if (stat_pub < 0) { - warnx("ERROR: orb_advertise for topic vehicle_status failed.\n"); + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); exit(ERROR); } @@ -1396,6 +1402,7 @@ int commander_thread_main(int argc, char *argv[]) /* now initialized */ commander_initialized = true; + thread_running = true; uint64_t start_time = hrt_absolute_time(); uint64_t failsave_ll_start_time = 0; @@ -1403,7 +1410,6 @@ int commander_thread_main(int argc, char *argv[]) bool state_changed = true; bool param_init_forced = true; - while (!thread_should_exit) { /* Get current values */ From 4a15eef602528bb79a62838e033be989e5fa2b3f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Feb 2013 09:01:31 +1100 Subject: [PATCH 09/13] px4io: fixed signals for lower latency PWM output poll() is not interrupted by signals, whereas usleep() is --- apps/px4io/px4io.c | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 122f007543..56923a674b 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -214,28 +214,34 @@ int user_start(int argc, char *argv[]) debug("Failed to setup SIGUSR1 handler\n"); } - /* 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; + /* + run the mixer at ~50Hz, using signals to run it early if + need be + */ + uint64_t last_debug_time = 0; for (;;) { /* - if we are not scheduled for 10ms then reset the I2C bus + if we are not scheduled for 30ms then reset the I2C bus */ - hrt_call_after(&loop_overtime_call, 10000, (hrt_callout)loop_overtime, NULL); + hrt_call_after(&loop_overtime_call, 30000, (hrt_callout)loop_overtime, NULL); + + // we use usleep() instead of poll() as poll() is not + // interrupted by signals in nuttx, whereas usleep() is + usleep(20000); - 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", + if (hrt_absolute_time() - last_debug_time > 1000000) { + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, (unsigned)i2c_loop_resets); + last_debug_time = hrt_absolute_time(); } } } From e896944adcce3d0d5e333186a76b35850e5f9bc9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 19 Feb 2013 14:45:45 +1100 Subject: [PATCH 10/13] ms5611: try to measure the performance cost of I2C timeouts --- apps/drivers/ms5611/ms5611.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 30166828a5..44014d969b 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -144,6 +144,7 @@ private: orb_advert_t _baro_topic; perf_counter_t _sample_perf; + perf_counter_t _measure_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; @@ -274,6 +275,7 @@ MS5611::MS5611(int bus) : _msl_pressure(101325), _baro_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), + _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), _comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows")) { @@ -647,6 +649,8 @@ MS5611::measure() { int ret; + perf_begin(_measure_perf); + /* * In phase zero, request temperature; in other phases, request pressure. */ @@ -664,6 +668,8 @@ MS5611::measure() if (OK != ret) perf_count(_comms_errors); + perf_end(_measure_perf); + return ret; } @@ -689,6 +695,7 @@ MS5611::collect() ret = transfer(&cmd, 1, &data[0], 3); if (ret != OK) { perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } From 69d9265bc3a9883c3cc180fe54223dd3be9dc36d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Feb 2013 09:44:15 +0100 Subject: [PATCH 11/13] Adjusted stack size for commander app --- apps/commander/commander.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 3996572cf2..42ca10f55d 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1227,8 +1227,8 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn("commander", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 50, - 4000, + SCHED_PRIORITY_MAX - 40, + 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); From 4cde2754666cea6332bb8069da518d28226c9477 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Feb 2013 09:47:08 +0100 Subject: [PATCH 12/13] Switched I2C to interrupt driven mode --- nuttx/configs/px4fmu/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 5d6900ab35..130886aac2 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -369,7 +369,7 @@ CONFIG_I2C_WRITEREAD=y # I2C configuration # CONFIG_I2C=y -CONFIG_I2C_POLLED=y +CONFIG_I2C_POLLED=n CONFIG_I2C_TRANSFER=y CONFIG_I2C_TRACE=n CONFIG_I2C_RESET=y From be408451779dc53220ec94499a7acbe5ff3b8e7f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Feb 2013 12:19:03 +0100 Subject: [PATCH 13/13] Switched to debug statement which is more efficient regarding stack usage, only printing at debug level 2 or higher. --- apps/px4io/mixer.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 93bd4470f2..3ae2a3115e 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -301,7 +301,7 @@ mixer_handle_text(const void *buffer, size_t length) px4io_mixdata *msg = (px4io_mixdata *)buffer; - debug("mixer text %u", length); + isr_debug(2, "mixer text %u", length); if (length < sizeof(px4io_mixdata)) return; @@ -310,14 +310,14 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: - debug("reset"); + isr_debug(2, "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); + isr_debug(2, "append %d", length); /* check for overflow - this is really fatal */ /* XXX could add just what will fit & try to parse, then repeat... */ @@ -330,7 +330,7 @@ mixer_handle_text(const void *buffer, size_t length) memcpy(&mixer_text[mixer_text_length], msg->text, text_length); mixer_text_length += text_length; mixer_text[mixer_text_length] = '\0'; - debug("buflen %u", mixer_text_length); + isr_debug(2, "buflen %u", mixer_text_length); /* process the text buffer, adding new mixers as their descriptions can be parsed */ unsigned resid = mixer_text_length; @@ -342,7 +342,7 @@ mixer_handle_text(const void *buffer, size_t length) /* ideally, this should test resid == 0 ? */ r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; - debug("used %u", mixer_text_length - resid); + isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ if (resid > 0)