From 2c2c65d446f991b273ee1f275fd2bc8440f74844 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Feb 2013 22:26:04 +0100 Subject: [PATCH 01/65] corrected some wrong units (used in airspeed calculation) --- apps/systemlib/conversions.c | 2 +- apps/systemlib/conversions.h | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 2b8003e458..ac94252c57 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -150,5 +150,5 @@ void quat2rot(const float Q[4], float R[9]) float get_air_density(float static_pressure, float temperature_celsius) { - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN)); + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); } diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index c2987709bb..5d485b01f3 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -44,10 +44,10 @@ #include #include -#define CONSTANTS_ONE_G 9.80665f -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f -#define CONSTANTS_AIR_GAS_CONST 8.31432f -#define CONSTANTS_ABSOLUTE_NULL_KELVIN 273.15f +#define CONSTANTS_ONE_G 9.80665f // m/s^2 +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3 +#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K) +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C __BEGIN_DECLS From a704acc2a20936d7e6d6828ae0ddf2cf7dc3578b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Feb 2013 12:02:00 +0100 Subject: [PATCH 02/65] Out of memory warning, flash and RAM optimizations --- apps/px4io/controls.c | 1 - apps/px4io/dsm.c | 6 +-- apps/px4io/mixer.cpp | 67 +------------------------------- apps/px4io/px4io.c | 31 +++++++++++---- apps/px4io/sbus.c | 6 ++- nuttx/configs/px4io/io/defconfig | 6 +-- 6 files changed, 34 insertions(+), 83 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index b4a18bae6b..829cc76716 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -45,7 +45,6 @@ #include #include -//#define DEBUG #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index f0e8e0f322..1719cf58cb 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -248,18 +248,18 @@ dsm_guess_format(bool reset) if ((votes11 == 1) && (votes10 == 0)) { channel_shift = 11; - debug("DSM: detected 11-bit format"); + debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { channel_shift = 10; - debug("DSM: detected 10-bit format"); + debug("DSM: 10-bit format"); return; } /* call ourselves to reset our state ... we have to try again */ - debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); + debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); dsm_guess_format(true); } diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 3ae2a3115e..505bc8a699 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -157,71 +157,6 @@ mixer_tick(void) 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; - control_values = &system_state.fmu_channel_data[0]; - - } else if (system_state.rc_channels > 0) { - /* when override is on or the fmu is not available, but RC is present */ - control_count = system_state.rc_channels; - - sched_lock(); - - /* remap roll, pitch, yaw and throttle from RC specific to internal ordering */ - rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1]; - rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1]; - rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1]; - rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1]; - //rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1]; - - /* get the remaining channels, no remapping needed */ - for (unsigned i = 4; i < system_state.rc_channels; i++) { - rc_channel_data[i] = system_state.rc_channel_data[i]; - } - - /* scale the control inputs */ - rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / - (float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000; - - if (rc_channel_data[THROTTLE] > 2000) { - rc_channel_data[THROTTLE] = 2000; - } - - if (rc_channel_data[THROTTLE] < 1000) { - rc_channel_data[THROTTLE] = 1000; - } - - // lowsyslog("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", - // (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]), - // (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE])); - - control_values = &rc_channel_data[0]; - sched_unlock(); - } else { - /* we have no control input (no FMU, no RC) */ - - // XXX builtin failsafe would activate here - control_count = 0; - } - //lowsyslog("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]); - - /* this is for multicopters, etc. where manual override does not make sense */ - } else { - /* if the fmu is available whe are good */ - if (system_state.mixer_fmu_available) { - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - /* we better shut everything off */ - } else { - control_count = 0; - } - } -#endif - /* * Decide whether the servos should be armed right now. * @@ -301,7 +236,7 @@ mixer_handle_text(const void *buffer, size_t length) px4io_mixdata *msg = (px4io_mixdata *)buffer; - isr_debug(2, "mixer text %u", length); + isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) return; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 56923a674b..b4135aba5a 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -83,12 +83,12 @@ 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 + * 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]; +static char msg[NUM_MSG][40]; /* add a debug message to be printed on the console @@ -127,7 +127,7 @@ static void show_debug_messages(void) */ static void loop_overtime(void *arg) { - debug("RESETTING\n"); + lowsyslog("I2C RESET\n"); i2c_loop_resets++; i2c_dump(); i2c_reset(); @@ -136,7 +136,7 @@ static void loop_overtime(void *arg) static void wakeup_handler(int signo, siginfo_t *info, void *ucontext) { - // nothing to do - we just want poll() to return + /* nothing to do - we just want poll() to return */ } @@ -185,16 +185,31 @@ int user_start(int argc, char *argv[]) up_pwm_servo_init(0xff); /* start the flight control signal handler */ - task_create("FCon", + int ret = task_create("FCon", SCHED_PRIORITY_DEFAULT, 1024, (main_t)controls_main, NULL); struct mallinfo minfo = mallinfo(); - lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); + lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); - debug("debug_level=%u\n", (unsigned)debug_level); + /* not enough memory, lock down */ + if (ret != OK || minfo.mxordblk < 500) { + lowsyslog("ERR: not enough MEM"); + bool phase = false; + + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + + phase = !phase; + usleep(300000); + } /* start the i2c handler */ i2c_init(); @@ -211,7 +226,7 @@ int user_start(int argc, char *argv[]) sigfillset(&sa.sa_mask); sigdelset(&sa.sa_mask, SIGUSR1); if (sigaction(SIGUSR1, &sa, NULL) != OK) { - debug("Failed to setup SIGUSR1 handler\n"); + lowsyslog("SIGUSR1 init fail\n"); } /* diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index d199a9361c..073ddeabae 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -53,7 +53,7 @@ #include "debug.h" #define SBUS_FRAME_SIZE 25 -#define SBUS_INPUT_CHANNELS 18 +#define SBUS_INPUT_CHANNELS 16 static int sbus_fd = -1; @@ -239,7 +239,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) } /* decode switch channels if data fields are wide enough */ - if (chancount > 17) { + if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { + chancount = 18; + /* channel 17 (index 16) */ values[16] = (frame[23] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 5db94c6f11..87d5001615 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -475,12 +475,12 @@ CONFIG_ARCH_BZERO=n # timer structures to minimize dynamic allocations. Set to # zero for all dynamic allocations. # -CONFIG_MAX_TASKS=8 +CONFIG_MAX_TASKS=4 CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=4 +CONFIG_NPTHREAD_KEYS=2 CONFIG_NFILE_DESCRIPTORS=8 CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=32 +CONFIG_NAME_MAX=12 CONFIG_STDIO_BUFFER_SIZE=32 CONFIG_STDIO_LINEBUFFER=n CONFIG_NUNGET_CHARS=2 From 5aa5645fb060c13997dc6458b530acb551c6c53e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Feb 2013 12:02:58 +0100 Subject: [PATCH 03/65] Disabled MAVLink debug app --- nuttx/configs/px4fmu/nsh/appconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 289a721a1a..2bf3283c81 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -81,7 +81,7 @@ CONFIGURED_APPS += systemcmds/delay_test # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/debug_values -CONFIGURED_APPS += examples/px4_mavlink_debug +# CONFIGURED_APPS += examples/px4_mavlink_debug # Shared object broker; required by many parts of the system. CONFIGURED_APPS += uORB From 8c7e2546ed5222145a6d1745e77d01f7c21c24fc Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 00:09:37 -0800 Subject: [PATCH 04/65] Simplify the PX4IO main loop to cut down on memory consumption. --- apps/px4io/controls.c | 361 ++++++++++++++++--------------- apps/px4io/px4io.c | 115 ++++------ apps/px4io/px4io.h | 8 +- apps/px4io/registers.c | 4 - nuttx/configs/px4io/io/defconfig | 6 +- 5 files changed, 227 insertions(+), 267 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 829cc76716..cad368ae43 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -39,7 +39,6 @@ #include #include -#include #include #include @@ -53,21 +52,18 @@ static bool ppm_input(uint16_t *values, uint16_t *num_values); -void -controls_main(void) -{ - struct pollfd fds[2]; +static perf_counter_t c_gather_dsm; +static perf_counter_t c_gather_sbus; +static perf_counter_t c_gather_ppm; +void +controls_init(void) +{ /* DSM input */ - fds[0].fd = dsm_init("/dev/ttyS0"); - fds[0].events = POLLIN; + dsm_init("/dev/ttyS0"); /* S.bus input */ - fds[1].fd = sbus_init("/dev/ttyS2"); - fds[1].events = POLLIN; - - ASSERT(fds[0].fd >= 0); - ASSERT(fds[1].fd >= 0); + sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { @@ -82,200 +78,207 @@ controls_main(void) r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } - for (;;) { - /* run this loop at ~100Hz */ - int result = poll(fds, 2, 10); + c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); + c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); + c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm"); +} - ASSERT(result >= 0); +void +controls_tick() { - /* - * Gather R/C control inputs from supported sources. - * - * Note that if you're silly enough to connect more than - * one control input source, they're going to fight each - * other. Don't do that. - */ + /* + * Gather R/C control inputs from supported sources. + * + * Note that if you're silly enough to connect more than + * one control input source, they're going to fight each + * other. Don't do that. + */ - 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; + perf_begin(c_gather_dsm); + 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; + perf_end(c_gather_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; + perf_begin(c_gather_sbus); + 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; + perf_end(c_gather_sbus); - /* - * 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. - */ - 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; + /* + * 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. + */ + perf_begin(c_gather_ppm); + 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; + perf_end(c_gather_ppm); - ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); + ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - /* - * In some cases we may have received a frame, but input has still - * been lost. - */ - bool rc_input_lost = false; - /* - * If we received a new frame from any of the RC sources, process it. - */ - if (dsm_updated || sbus_updated || ppm_updated) { + /* + * In some cases we may have received a frame, but input has still + * been lost. + */ + bool rc_input_lost = false; - /* update RC-received timestamp */ - system_state.rc_channels_timestamp = hrt_absolute_time(); + /* + * If we received a new frame from any of the RC sources, process it. + */ + if (dsm_updated || sbus_updated || ppm_updated) { - /* record a bitmask of channels assigned */ - unsigned assigned_channels = 0; + /* update RC-received timestamp */ + system_state.rc_channels_timestamp = hrt_absolute_time(); - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + /* record a bitmask of channels assigned */ + unsigned assigned_channels = 0; - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - uint16_t raw = r_raw_rc_values[i]; + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - /* 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]; - } + uint16_t raw = r_raw_rc_values[i]; - /* 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]; - - 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); + /* 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]; } - } - /* 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; - } + /* 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]; - /* - * 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; - } + int16_t scaled = raw; - /* - * Export the valid channel bitmap - */ - r_rc_valid = assigned_channels; + /* 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 haven't seen any new control data in 200ms, assume we - * have lost input. - */ - if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { - rc_input_lost = 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); - } - - /* - * 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; - } - - /* - * Check for manual override. + * If we got an update with zero channels, treat it as + * a loss of input. * - * 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. + * This might happen if a protocol-based receiver returns an update + * that contains no channels that we have mapped. */ - 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; - } + 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. + */ + if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + rc_input_lost = 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); + } + + /* + * 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; + } + + /* + * 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. + */ + 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; + } } } diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index b4135aba5a..5892646612 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -68,14 +68,8 @@ static struct hrt_call serial_dma_call; volatile uint8_t debug_level = 0; volatile uint32_t i2c_loop_resets = 0; -struct hrt_call loop_overtime_call; - -// this allows wakeup of the main task via a signal -static pid_t daemon_pid; - - /* - a set of debug buffers to allow us to send debug information from ISRs + * a set of debug buffers to allow us to send debug information from ISRs */ static volatile uint32_t msg_counter; @@ -91,9 +85,10 @@ static volatile uint8_t msg_next_out, msg_next_in; static char msg[NUM_MSG][40]; /* - add a debug message to be printed on the console + * add a debug message to be printed on the console */ -void isr_debug(uint8_t level, const char *fmt, ...) +void +isr_debug(uint8_t level, const char *fmt, ...) { if (level > debug_level) { return; @@ -107,9 +102,10 @@ void isr_debug(uint8_t level, const char *fmt, ...) } /* - show all pending debug messages + * show all pending debug messages */ -static void show_debug_messages(void) +static void +show_debug_messages(void) { if (msg_counter != last_msg_counter) { uint32_t n = msg_counter - last_msg_counter; @@ -122,36 +118,9 @@ static void show_debug_messages(void) } } -/* - catch I2C lockups - */ -static void loop_overtime(void *arg) +int +user_start(int argc, char *argv[]) { - lowsyslog("I2C RESET\n"); - i2c_loop_resets++; - i2c_dump(); - i2c_reset(); - hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL); -} - -static void wakeup_handler(int signo, siginfo_t *info, void *ucontext) -{ - /* nothing to do - we just want poll() to return */ -} - - -/* - wakeup the main task using a signal - */ -void daemon_wakeup(void) -{ - kill(daemon_pid, SIGUSR1); -} - -int user_start(int argc, char *argv[]) -{ - daemon_pid = getpid(); - /* run C++ ctors before we go any further */ up_cxxinitialize(); @@ -184,18 +153,27 @@ int user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); - /* start the flight control signal handler */ - int ret = task_create("FCon", - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)controls_main, - NULL); + /* initialise the control inputs */ + controls_init(); + + /* start the i2c handler */ + i2c_init(); + + /* add a performance counter for mixing */ + perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); + + /* add a performance counter for controls */ + perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); + + /* and one for measuring the loop rate */ + perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); struct mallinfo minfo = mallinfo(); lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); +#if 0 /* not enough memory, lock down */ - if (ret != OK || minfo.mxordblk < 500) { + if (minfo.mxordblk < 500) { lowsyslog("ERR: not enough MEM"); bool phase = false; @@ -210,46 +188,33 @@ int user_start(int argc, char *argv[]) phase = !phase; usleep(300000); } +#endif - /* start the i2c handler */ - i2c_init(); - - /* add a performance counter for mixing */ - perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); - - /* - * setup a null handler for SIGUSR1 - we will use this for wakeup from poll() + /* + * Run everything in a tight loop. */ - struct sigaction sa; - memset(&sa, 0, sizeof(sa)); - sa.sa_sigaction = wakeup_handler; - sigfillset(&sa.sa_mask); - sigdelset(&sa.sa_mask, SIGUSR1); - if (sigaction(SIGUSR1, &sa, NULL) != OK) { - lowsyslog("SIGUSR1 init fail\n"); - } - /* - 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 30ms then reset the I2C bus - */ - 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); + /* track the rate at which the loop is running */ + perf_count(loop_perf); + /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); + /* kick the control inputs */ + perf_begin(controls_perf); + controls_tick(); + perf_end(controls_perf); + + /* check for debug activity */ show_debug_messages(); - if (hrt_absolute_time() - last_debug_time > 1000000) { + + /* post debug state at ~1Hz */ + if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", (unsigned)debug_level, (unsigned)r_status_flags, diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index cd5977258d..22993fb52f 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -170,7 +170,8 @@ extern uint16_t adc_measure(unsigned channel); * * Input functions return true when they receive an update from the RC controller. */ -extern void controls_main(void); +extern void controls_init(void); +extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern int sbus_init(const char *device); @@ -179,11 +180,6 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/** - * Wake up mixer. - */ -void daemon_wakeup(void); - /* send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index ec1980aaaf..5fb90b9b0c 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -203,8 +203,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; - // wake up daemon to trigger mixer - daemon_wakeup(); break; /* handle raw PWM input */ @@ -224,8 +222,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; - // wake up the main thread to trigger mixer - daemon_wakeup(); break; /* handle setup for servo failsafe values */ diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 87d5001615..1145fb349f 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -401,11 +401,11 @@ CONFIG_SCHED_ATEXIT=n CONFIG_DISABLE_CLOCK=n CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=n +CONFIG_DISABLE_POLL=y # # Misc libc settings @@ -541,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n CONFIG_CUSTOM_STACK=n CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=800 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 CONFIG_HEAP_BASE= From 2707d2c1dde65dfb9ba48258994badb4b57f9627 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Feb 2013 16:01:08 +0100 Subject: [PATCH 05/65] more fixes for the airspeed readout --- apps/sensors/sensors.cpp | 25 ++++----- apps/systemlib/airspeed.c | 40 +++++++++----- apps/systemlib/airspeed.h | 69 ++++++++++++------------ apps/uORB/topics/differential_pressure.h | 4 +- 4 files changed, 77 insertions(+), 61 deletions(-) diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d8d200ea9d..c29910dccd 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -993,7 +993,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* look for battery channel */ for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - + if (ret >= (int)sizeof(buf_adc[0])) { if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1025,8 +1025,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - - float voltage = buf_adc[i].am_data / 4096.0f; + float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on @@ -1034,21 +1033,23 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f) { - float pres_raw = fabsf(voltage - (3.3f / 2.0f)); - float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f; +// float pres_raw = fabsf(voltage - (3.3f / 2.0f)); +// float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f; + //XXX depends on sensor used..., where are the above numbers from? + float diff_pres_pa = fabsf(voltage - 2.5f) * 1000.0f; //for MPXV7002DP //xxx: need an offset calibration - float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure, - _barometer.pressure, _barometer.temperature - 5.0f); + float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f, + _barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa // XXX HACK - true temperature is much less than indicated temperature in baro, // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - float airspeed_indicated = calc_indicated_airspeed(pres_mbar + _barometer.pressure, - _barometer.pressure, _barometer.temperature - 5.0f); - // XXX HACK + float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa); + + //printf("voltage: %.4f, diff_pres_pa %.4f, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)airspeed_indicated, (double)airspeed_true); _differential_pressure.timestamp = hrt_absolute_time(); _differential_pressure.static_pressure_mbar = _barometer.pressure; - _differential_pressure.differential_pressure_mbar = pres_mbar; + _differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f; _differential_pressure.temperature_celcius = _barometer.temperature; _differential_pressure.indicated_airspeed_m_s = airspeed_indicated; _differential_pressure.true_airspeed_m_s = airspeed_true; @@ -1064,7 +1065,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } _last_adc = hrt_absolute_time(); - break; + //break; } } } diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 5c68f8ea52..32943b2f5b 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -40,14 +40,25 @@ * */ -#include "math.h" +#include +#include #include "conversions.h" #include "airspeed.h" -float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature) +/** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param differential_pressure total_ pressure - static pressure + * @return indicated airspeed in m/s + */ +float calc_indicated_airspeed(float differential_pressure) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } /** @@ -55,14 +66,14 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param speed current indicated airspeed + * @param speed_indicated current indicated airspeed * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature) +float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) { - return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature)); + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius)); } /** @@ -70,12 +81,17 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param total_pressure pressure inside the pitot/prandl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature) +float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature)); + float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || isnan(density)) { + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; + printf("Invalid air density, using density at sea level\n"); + } + return sqrtf((2.0f*(total_pressure - static_pressure)) / density); } diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h index b1beb79ae0..0884989439 100644 --- a/apps/systemlib/airspeed.h +++ b/apps/systemlib/airspeed.h @@ -48,43 +48,42 @@ __BEGIN_DECLS -/** - * Calculate indicated airspeed. - * - * Note that the indicated airspeed is not the true airspeed because it - * lacks the air density compensation. Use the calc_true_airspeed functions to get - * the true airspeed. - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return indicated airspeed in m/s - */ -__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @return indicated airspeed in m/s + */ + __EXPORT float calc_indicated_airspeed(float differential_pressure); -/** - * Calculate true airspeed from indicated airspeed. - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param speed current indicated airspeed - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature); + /** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius); -/** - * Directly calculate true airspeed - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); __END_DECLS diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h index fd7670cbc8..78a37fdf46 100644 --- a/apps/uORB/topics/differential_pressure.h +++ b/apps/uORB/topics/differential_pressure.h @@ -49,7 +49,7 @@ */ /** - * Battery voltages and status + * Differential pressure and airspeed */ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ @@ -67,4 +67,4 @@ struct differential_pressure_s { /* register this as object request broker structure */ ORB_DECLARE(differential_pressure); -#endif \ No newline at end of file +#endif From a11a71ec9c1b04cd5ca515862605008038804c8d Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 10:54:22 -0800 Subject: [PATCH 06/65] Hotfix: discard NUL characters in readline, rather than faking EOF on the console. --- apps/system/readline/readline.c | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/apps/system/readline/readline.c b/apps/system/readline/readline.c index bac7eee8c5..a8240a62a9 100644 --- a/apps/system/readline/readline.c +++ b/apps/system/readline/readline.c @@ -126,7 +126,7 @@ static inline int readline_rawgetc(int infd) * error occurs). */ - do + for (;;) { /* Read one character from the incoming stream */ @@ -154,13 +154,21 @@ static inline int readline_rawgetc(int infd) { return -errcode; } + + continue; } + + else if (buffer == '\0') + { + /* Ignore NUL characters, since they look like EOF to our caller */ + + continue; + } + + /* Success, return the character that was read */ + + return (int)buffer; } - while (nread < 1); - - /* On success, return the character that was read */ - - return (int)buffer; } /**************************************************************************** From 8740349545ab7fea4cbb231535de234bb0939764 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Feb 2013 20:13:45 +0100 Subject: [PATCH 07/65] Removed 1 Hz output --- apps/examples/kalman_demo/KalmanNav.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index db851221b2..955e77b3e4 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -247,8 +247,8 @@ void KalmanNav::update() // output if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz, miss #: %4d\n", - _navFrames / 10, _miss / 10); + //printf("nav: %4d Hz, miss #: %4d\n", + // _navFrames / 10, _miss / 10); _navFrames = 0; _miss = 0; } From 35369471db820ba79e9803d4a48ea74ad6843e86 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Feb 2013 20:24:21 +0100 Subject: [PATCH 08/65] working on better status reporting, removed unneeded fake PWM generation from FMU --- apps/drivers/px4io/px4io.cpp | 32 +++++++++++++++++++++++--------- apps/sensors/sensors.cpp | 30 ------------------------------ 2 files changed, 23 insertions(+), 39 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index adb894371f..a06a2575eb 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -96,6 +96,8 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); + void print_status(); + private: // XXX unsigned _max_actuators; @@ -459,7 +461,7 @@ PX4IO::init() PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); - /* publish RC config to IO */ + /* publish RC config to IO */ ret = io_set_rc_config(); if (ret != OK) { log("failed to update RC input config"); @@ -1141,18 +1143,28 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) } while (buflen > 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) + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { + debug("mixer upload OK"); return 0; - - debug("mixer rejected by IO"); + } else { + debug("mixer rejected by IO"); + } /* load must have failed for some reason */ return -EINVAL; } +void +PX4IO::print_status() +{ + printf("\tRC status:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_OK) ? "OK" : "LOST"); + if (_status & PX4IO_P_STATUS_FLAGS_RC_OK) { + printf("\tRC type:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? "S.BUS" : ((_status & PX4IO_P_STATUS_FLAGS_RC_DSM) ? "DSM" : "PPM")); + // printf("\tRC chans:\t%d\n", xxx); + } +} + int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { @@ -1294,7 +1306,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } default: - /* not a recognised value */ + /* not a recognized value */ ret = -ENOTTY; } @@ -1458,10 +1470,12 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) + if (g_dev != nullptr) { printf("[px4io] loaded\n"); - else + g_dev->print_status(); + } else { printf("[px4io] not loaded\n"); + } exit(0); } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d8d200ea9d..edff8828f7 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1074,36 +1074,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw) void Sensors::ppm_poll() { - /* fake low-level driver, directly pulling from driver variables */ - static orb_advert_t rc_input_pub = -1; - struct rc_input_values raw; - - raw.timestamp = ppm_last_valid_decode; - /* we are accepting this message */ - _ppm_last_valid = ppm_last_valid_decode; - - /* - * relying on two decoded channels is very noise-prone, - * in particular if nothing is connected to the pins. - * requiring a minimum of four channels - */ - if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) { - - for (unsigned i = 0; i < ppm_decoded_channels; i++) { - raw.values[i] = ppm_buffer[i]; - } - - raw.channel_count = ppm_decoded_channels; - - /* publish to object request broker */ - if (rc_input_pub <= 0) { - rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw); - - } else { - orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); - } - } - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ bool rc_updated; From 923a7cc505971ab6d04115d18108152f62d46283 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:41:26 -0800 Subject: [PATCH 09/65] Add an interrupt-safe way of comparing a timestamp with the current time. Add an interrupt-safe way of storing the current time into a timestamp. --- apps/drivers/drv_hrt.h | 16 ++++++++++++++++ apps/drivers/stm32/drv_hrt.c | 30 ++++++++++++++++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h index 3b493a81a8..2e30725bdc 100644 --- a/apps/drivers/drv_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -91,6 +91,22 @@ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts); */ __EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); +/* + * Compute the delta between a timestamp taken in the past + * and now. + * + * This function is safe to use even if the timestamp is updated + * by an interrupt during execution. + */ +__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); + +/* + * Store the absolute time in an interrupt-safe fashion. + * + * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. + */ +__EXPORT extern hrt_absolute_time hrt_store_absolute_time(volatile hrt_abstime *now); + /* * Call callout(arg) after delay has elapsed. * diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index cd9cb45b1c..0df2a8b748 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -645,6 +645,36 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime) ts->tv_nsec = abstime * 1000; } +/* + * Compare a time value with the current time. + */ +hrt_abstime +hrt_elapsed_time(const volatile hrt_abstime *then) +{ + irqstate_t flags = irqsave(); + + hrt_abstime delta = hrt_absolute_time() - *then; + + irqrestore(flags); + + return delta; +} + +/* + * Store the absolute time in an interrupt-safe fashion + */ +hrt_absolute_time +hrt_store_absolute_time(volatile hrt_abstime *now) +{ + irqstate_t flags = irqsave(); + + hrt_abstime ts = hrt_absolute_time(); + + irqrestore(flags); + + return ts; +} + /* * Initalise the high-resolution timing module. */ From f245d6b1a7edaa3b403007b704b4d54ecb7f3737 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:42:34 -0800 Subject: [PATCH 10/65] Use hrt_elapsed_time() in cases where we can't be sure the timestamp won't change under us. --- apps/px4io/adc.c | 2 +- apps/px4io/controls.c | 4 ++-- apps/px4io/mixer.cpp | 2 +- apps/px4io/px4io.h | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c index e06b269dc0..670b8d635d 100644 --- a/apps/px4io/adc.c +++ b/apps/px4io/adc.c @@ -154,7 +154,7 @@ adc_measure(unsigned channel) while (!(rSR & ADC_SR_EOC)) { /* never spin forever - this will give a bogus result though */ - if ((hrt_absolute_time() - now) > 1000) { + if (hrt_elapsed_time(&now) > 1000) { debug("adc timeout"); break; } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index cad368ae43..6b51647569 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -218,7 +218,7 @@ controls_tick() { * If we haven't seen any new control data in 200ms, assume we * have lost input. */ - if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { rc_input_lost = true; /* clear the input-kind flags here */ @@ -294,7 +294,7 @@ ppm_input(uint16_t *values, uint16_t *num_values) * If we have received a new PPM frame within the last 200ms, accept it * and then invalidate it. */ - if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) { + if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) { /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 505bc8a699..77f28cd7a6 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -88,7 +88,7 @@ void mixer_tick(void) { /* 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) { + if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index 22993fb52f..7b4b07c2c7 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -105,12 +105,12 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ */ struct sys_state_s { - uint64_t rc_channels_timestamp; + volatile uint64_t rc_channels_timestamp; /** * Last FMU receive time, in microseconds since system boot */ - uint64_t fmu_data_received_time; + volatile uint64_t fmu_data_received_time; }; From 186d3297228e4fbf34bb71545d0cdbac08e78fb3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:47:56 -0800 Subject: [PATCH 11/65] Fix search-and-replace error. --- apps/drivers/drv_hrt.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h index 2e30725bdc..0a64d69c34 100644 --- a/apps/drivers/drv_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -105,7 +105,7 @@ __EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); * * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. */ -__EXPORT extern hrt_absolute_time hrt_store_absolute_time(volatile hrt_abstime *now); +__EXPORT extern hrt_abstime_time hrt_store_absolute_time(volatile hrt_abstime *now); /* * Call callout(arg) after delay has elapsed. From ccbd5a6372ff2ec40b1f014571712c4554b01e71 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:48:52 -0800 Subject: [PATCH 12/65] No, really fix it this time. --- apps/drivers/drv_hrt.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h index 0a64d69c34..8a99eeca79 100644 --- a/apps/drivers/drv_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -105,7 +105,7 @@ __EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); * * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. */ -__EXPORT extern hrt_abstime_time hrt_store_absolute_time(volatile hrt_abstime *now); +__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now); /* * Call callout(arg) after delay has elapsed. From 3d53b1d551d2ff901b68503e55167be03123b067 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 11:50:57 -0800 Subject: [PATCH 13/65] Fix it here, too. --- apps/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 0df2a8b748..bb67d5e6d2 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -663,7 +663,7 @@ hrt_elapsed_time(const volatile hrt_abstime *then) /* * Store the absolute time in an interrupt-safe fashion */ -hrt_absolute_time +hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now) { irqstate_t flags = irqsave(); From 72603207a1b16b628f153d195788171054cfc047 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Feb 2013 21:12:25 +0100 Subject: [PATCH 14/65] Fixed formatting of status printing --- apps/drivers/px4io/px4io.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index a06a2575eb..d695c4f825 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1158,11 +1158,13 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) void PX4IO::print_status() { - printf("\tRC status:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_OK) ? "OK" : "LOST"); + printf("\tRC status:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_OK) ? "OK" : "FAIL"); if (_status & PX4IO_P_STATUS_FLAGS_RC_OK) { printf("\tRC type:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? "S.BUS" : ((_status & PX4IO_P_STATUS_FLAGS_RC_DSM) ? "DSM" : "PPM")); // printf("\tRC chans:\t%d\n", xxx); } + //printf("\tRC Config:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_CONFIG_OK) ? "OK" : "FAIL"); + printf("\tFMU link:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_FMU_OK) ? "OK" : "FAIL"); } int From c0a852dab48e55aa12c995adc6dc0c32aa9a7ac3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Feb 2013 21:57:38 +0100 Subject: [PATCH 15/65] airspeed (pitot) offset calibration --- apps/commander/commander.c | 96 ++++++++++++++++++++++++ apps/sensors/sensor_params.c | 2 + apps/sensors/sensors.cpp | 13 +++- apps/systemlib/airspeed.c | 20 ++++- apps/uORB/topics/differential_pressure.h | 5 +- apps/uORB/topics/vehicle_status.h | 1 + 6 files changed, 130 insertions(+), 7 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 42ca10f55d..41f4e56742 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -80,6 +80,7 @@ #include #include #include +#include #include #include @@ -785,6 +786,79 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } +void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + + mavlink_log_info(mavlink_fd, "keep it still"); + /* set to accel calibration mode */ + status->flag_preflight_airspeed_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + const int calibration_count = 2500; + + int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + + int calibration_counter = 0; + float airspeed_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); + airspeed_offset += differential_pressure.voltage; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + airspeed_offset = airspeed_offset / calibration_count; + + if (isfinite(airspeed_offset)) { + + if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + /* exit airspeed calibration mode */ + status->flag_preflight_airspeed_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); + + close(sub_differential_pressure); +} + void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) @@ -980,6 +1054,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta handled = true; } + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol + /* transition to calibration state */ + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + + if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + mavlink_log_info(mavlink_fd, "CMD starting airspeed cal"); + tune_confirm(); + do_airspeed_calibration(status_pub, ¤t_status); + tune_confirm(); + mavlink_log_info(mavlink_fd, "CMD finished airspeed cal"); + do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal"); + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + /* none found */ if (!handled) { //warnx("refusing unsupported calibration request\n"); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 9f23ebbbab..a1ef9d136e 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -64,6 +64,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f); + PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index c29910dccd..f77bc9b8b6 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -187,6 +187,7 @@ private: float mag_scale[3]; float accel_offset[3]; float accel_scale[3]; + float airspeed_offset; int rc_type; @@ -235,6 +236,7 @@ private: param_t accel_scale[3]; param_t mag_offset[3]; param_t mag_scale[3]; + param_t airspeed_offset; param_t rc_map_roll; param_t rc_map_pitch; @@ -480,6 +482,9 @@ Sensors::Sensors() : _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); + /*Airspeed offset */ + _parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF"); + _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); /* fetch initial parameter values */ @@ -687,6 +692,9 @@ Sensors::parameters_update() param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1])); param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2])); + /* Airspeed offset */ + param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset)); + /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { warnx("Failed updating voltage scaling param"); @@ -1036,7 +1044,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) // float pres_raw = fabsf(voltage - (3.3f / 2.0f)); // float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f; //XXX depends on sensor used..., where are the above numbers from? - float diff_pres_pa = fabsf(voltage - 2.5f) * 1000.0f; //for MPXV7002DP //xxx: need an offset calibration + float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f, _barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa @@ -1045,7 +1053,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa); - //printf("voltage: %.4f, diff_pres_pa %.4f, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)airspeed_indicated, (double)airspeed_true); + //printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true); _differential_pressure.timestamp = hrt_absolute_time(); _differential_pressure.static_pressure_mbar = _barometer.pressure; @@ -1053,6 +1061,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _differential_pressure.temperature_celcius = _barometer.temperature; _differential_pressure.indicated_airspeed_m_s = airspeed_indicated; _differential_pressure.true_airspeed_m_s = airspeed_true; + _differential_pressure.voltage = voltage; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 32943b2f5b..382df2ee4f 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -58,7 +58,13 @@ */ float calc_indicated_airspeed(float differential_pressure) { - return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + + if (differential_pressure > 0) { + return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { + return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } + } /** @@ -89,9 +95,17 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { float density = get_air_density(static_pressure, temperature_celsius); - if (density < 0.0001f || isnan(density)) { + if (density < 0.0001f || !isfinite(density)) { density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; printf("Invalid air density, using density at sea level\n"); } - return sqrtf((2.0f*(total_pressure - static_pressure)) / density); + + float pressure_difference = total_pressure - static_pressure; + + if(pressure_difference > 0) { + return sqrtf((2.0f*(pressure_difference)) / density); + } else + { + return -sqrtf((2.0f*fabs(pressure_difference)) / density); + } } diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h index 78a37fdf46..d5e4bf37ef 100644 --- a/apps/uORB/topics/differential_pressure.h +++ b/apps/uORB/topics/differential_pressure.h @@ -52,12 +52,13 @@ * Differential pressure and airspeed */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float static_pressure_mbar; /**< Static / environment pressure */ float differential_pressure_mbar; /**< Differential pressure reading */ float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */ }; /** diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 06b4c5ca5c..ae18ba050e 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -173,6 +173,7 @@ struct vehicle_status_s 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_accel_calibration; + bool flag_preflight_airspeed_calibration; bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ From 858460c863ca496e774a616698acf0eb94431dea Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 13:40:46 -0800 Subject: [PATCH 16/65] Extended PX4IO status dump --- apps/drivers/px4io/px4io.cpp | 101 ++++++++++++++++++++++++++++++++--- 1 file changed, 93 insertions(+), 8 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index d695c4f825..2f5d4cf89a 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -281,14 +281,15 @@ PX4IO::PX4IO() : _max_relays(0), _max_transfer(16), /* sensible default */ _update_interval(0), - _status(0), - _alarms(0), _task(-1), _task_should_exit(false), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), + _status(0), + _alarms(0), _t_actuators(-1), _t_armed(-1), _t_vstatus(-1), + _t_param(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), @@ -814,6 +815,8 @@ PX4IO::io_handle_alarms(uint16_t alarms) /* set new alarms state */ _alarms = alarms; + + return 0; } int @@ -1158,13 +1161,95 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) void PX4IO::print_status() { - printf("\tRC status:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_OK) ? "OK" : "FAIL"); - if (_status & PX4IO_P_STATUS_FLAGS_RC_OK) { - printf("\tRC type:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? "S.BUS" : ((_status & PX4IO_P_STATUS_FLAGS_RC_DSM) ? "DSM" : "PPM")); - // printf("\tRC chans:\t%d\n", xxx); + /* basic configuration */ + printf("protocol %u software %u bootloader %u buffer %uB\n", + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + + /* 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); + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n", + flags, + ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((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) ? " DSM" : ""), + ((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_PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC")); + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + printf("alarms 0x%04x%s%s%s%s%s%s\n", + alarms, + ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : "")); + printf("vbatt %u ibatt %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); + printf("actuators"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf("\n"); + printf("servos"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + printf("\n"); + uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); + printf("%d 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"); + uint16_t valid_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RC_VALID); + printf("valid R/C inputs 0x%04x", valid_inputs); + for (unsigned i = 0; i < _max_rc_input; i++) { + if (valid_inputs && (1 << i)) + printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)); } - //printf("\tRC Config:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_RC_CONFIG_OK) ? "OK" : "FAIL"); - printf("\tFMU link:\t%s\n", (_status & PX4IO_P_STATUS_FLAGS_FMU_OK) ? "OK" : "FAIL"); + printf("\n"); + uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); + printf("ADC inputs"); + for (unsigned i = 0; i < adc_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + printf("\n"); + + /* setup */ + printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); + uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); + printf("arming 0x%04x%s%s%s\n", + ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? "ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? "MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? "VECTOR_FLIGHT_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? "INAIR_RESTART_OK" : "")); + printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); + printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); + printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\n"); } int From e818bcbfc2a91b7716ef782d15f2b8bf9f644419 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 13:58:52 -0800 Subject: [PATCH 17/65] Fix a wrong register read for the mapped channel mask --- apps/drivers/px4io/px4io.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2f5d4cf89a..18350718ca 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1211,14 +1211,14 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); printf("\n"); uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); - printf("%d R/C inputs", raw_inputs); + 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"); - uint16_t valid_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RC_VALID); - printf("valid R/C inputs 0x%04x", valid_inputs); + uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); + printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { - if (valid_inputs && (1 << i)) + if (mapped_inputs && (1 << i)) printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)); } printf("\n"); @@ -1231,11 +1231,12 @@ PX4IO::print_status() /* setup */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s\n", - ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? "ARM_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? "MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? "VECTOR_FLIGHT_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? "INAIR_RESTART_OK" : "")); + printf("arming 0x%04x%s%s%s%s\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE), From 776cf6093c97a6bb4c8fe9543b91c7039708fc85 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 14:06:28 -0800 Subject: [PATCH 18/65] && -> & --- apps/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 18350718ca..2488f8022b 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1218,7 +1218,7 @@ PX4IO::print_status() uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { - if (mapped_inputs && (1 << i)) + if (mapped_inputs & (1 << i)) printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)); } printf("\n"); From 93f6edfe64780f1132cf8cb44afe3c7f1477f94e Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 14:30:56 -0800 Subject: [PATCH 19/65] Fix reporting of R/C input config --- apps/drivers/px4io/px4io.cpp | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2488f8022b..3872d7201f 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1228,7 +1228,7 @@ PX4IO::print_status() printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); printf("\n"); - /* setup */ + /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s\n", @@ -1251,6 +1251,20 @@ PX4IO::print_status() for (unsigned i = 0; i < _max_actuators; i++) printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, 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" : "")); + } } int From f35c5d600aa1d936207e3e6988058093dccacdf7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 14:32:04 -0800 Subject: [PATCH 20/65] Don't mask out the enable bit when accepting R/C input config updates. --- apps/px4io/registers.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 5fb90b9b0c..5ec2f7258b 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -165,8 +165,8 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; */ 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 +/* valid options */ +#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) /* * PAGE 104 uses r_page_servos. From dc74eeb421bce204a3064bcc60d524bf3fb53ab2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:31:01 -0800 Subject: [PATCH 21/65] Report the control values from the FMU in the status output. Count them separately from the actuators. --- apps/drivers/px4io/px4io.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 3872d7201f..a3b9957dd5 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -101,6 +101,7 @@ public: private: // XXX unsigned _max_actuators; + unsigned _max_controls; unsigned _max_rc_input; unsigned _max_relays; unsigned _max_transfer; @@ -277,6 +278,7 @@ PX4IO *g_dev; PX4IO::PX4IO() : I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), + _max_controls(0), _max_rc_input(0), _max_relays(0), _max_transfer(16), /* sensible default */ @@ -342,6 +344,7 @@ PX4IO::init() /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); + _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_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); @@ -637,11 +640,11 @@ PX4IO::io_set_control_state() orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &controls); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_controls; 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); + return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } int @@ -1247,9 +1250,9 @@ PX4IO::print_status() io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); - printf("failsafe"); - for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + 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; @@ -1265,6 +1268,10 @@ PX4IO::print_status() ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\n"); } int From 3d9901dfaf687e375569cbc3256b818ff01721c6 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:31:40 -0800 Subject: [PATCH 22/65] If we have seen control input from FMU, update the FMU_OK status flag. --- apps/px4io/mixer.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 77f28cd7a6..ed74cb3d30 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -94,9 +94,13 @@ mixer_tick(void) 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; + + /* XXX this is questionable - vehicle may not make sense for direct control */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + } else { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; } source = MIX_FAILSAFE; From 345b1a091554c92aa2d3e8e8df2b91cba2431aa5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:55:38 -0800 Subject: [PATCH 23/65] Print mapped R/C inputs as signed values (since they are zero-relative) --- apps/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index a3b9957dd5..4006f88cf0 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1222,7 +1222,7 @@ PX4IO::print_status() printf("mapped R/C inputs 0x%04x", mapped_inputs); for (unsigned i = 0; i < _max_rc_input; i++) { if (mapped_inputs & (1 << i)) - printf(" %u:%u", i, io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)); + printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); } printf("\n"); uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); From 6ac7e8b7e4662c297e02ffc43e2cd52126753fa2 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 15:56:02 -0800 Subject: [PATCH 24/65] Scale R/C inputs around the preset center, not the nominal center. --- apps/px4io/controls.c | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 6b51647569..37872d3569 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -119,7 +119,6 @@ controls_tick() { ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - /* * In some cases we may have received a frame, but input has still * been lost. @@ -168,8 +167,8 @@ controls_tick() { int16_t scaled = raw; - /* adjust to zero-relative (-500..500) */ - scaled -= 1500; + /* adjust to zero-relative around center */ + scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; /* scale to fixed-point representation (-10000..10000) */ scaled *= 20; From e6228355557aa7e06551711fc5a5a2127ca683f3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 24 Feb 2013 16:20:04 -0800 Subject: [PATCH 25/65] Bump the task stack up to 1200 bytes to give the mixer loader some headroom. This addresses the last reported issue with this branch. --- apps/px4io/controls.c | 2 +- nuttx/configs/px4io/io/defconfig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 37872d3569..21b4edcc37 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -167,7 +167,7 @@ controls_tick() { int16_t scaled = raw; - /* adjust to zero-relative around center */ + /* adjust to zero-relative around center (nominal -500..500) */ scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; /* scale to fixed-point representation (-10000..10000) */ diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 1145fb349f..bb937cf4ee 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -541,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n CONFIG_CUSTOM_STACK=n CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=800 +CONFIG_USERMAIN_STACKSIZE=1200 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 CONFIG_HEAP_BASE= From 5cc1e30e4fea92f32004288d8511de7e63c0c506 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Feb 2013 08:31:43 +0100 Subject: [PATCH 26/65] Corrected assertion range --- apps/px4io/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 21b4edcc37..3cc7140de5 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -173,8 +173,8 @@ controls_tick() { /* scale to fixed-point representation (-10000..10000) */ scaled *= 20; - ASSERT(scaled >= -15000); - ASSERT(scaled <= 15000); + ASSERT(scaled >= -50000); + ASSERT(scaled <= 50000); if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; From 2284e668ebab8fd452ecb5ca86d386e599f19ff1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Feb 2013 08:53:00 +0100 Subject: [PATCH 27/65] Removed bound checking assertions --- apps/px4io/controls.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 3cc7140de5..dabdde0af9 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -173,9 +173,6 @@ controls_tick() { /* scale to fixed-point representation (-10000..10000) */ scaled *= 20; - ASSERT(scaled >= -50000); - ASSERT(scaled <= 50000); - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; From ca794265c6a33f5aa9de87d97a222ed331c59aec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Feb 2013 09:07:13 +0100 Subject: [PATCH 28/65] Fixed input indexing, stupid 1-based indices on the GCS side (MP/QGC) caused confusion --- apps/drivers/px4io/px4io.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 4006f88cf0..2611c4e9c7 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -694,21 +694,26 @@ PX4IO::io_set_rc_config() for (unsigned i = 0; i < _max_rc_input; i++) input_map[i] = -1; + /* + * NOTE: The indices for mapped channels are 1-based + * for compatibility reasons with existing + * autopilots / GCS'. + */ param_get(param_find("RC_MAP_ROLL"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 0; + input_map[ichan - 1] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 1; + input_map[ichan - 1] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 2; + input_map[ichan - 1] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 3; + input_map[ichan - 1] = 3; ichan = 4; for (unsigned i = 0; i < _max_rc_input; i++) From 3f674ba78cc8af2be8b8062f9366629b6c2a34be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Feb 2013 14:34:53 +0100 Subject: [PATCH 29/65] fixed a typo --- apps/systemlib/airspeed.c | 2 +- apps/systemlib/airspeed.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 382df2ee4f..d7096d597d 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -87,7 +87,7 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param total_pressure pressure inside the pitot/prandl tube + * @param total_pressure pressure inside the pitot/prandtl tube * @param static_pressure pressure at the side of the tube/airplane * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h index 0884989439..def53f0c1b 100644 --- a/apps/systemlib/airspeed.h +++ b/apps/systemlib/airspeed.h @@ -78,7 +78,7 @@ * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param total_pressure pressure inside the pitot/prandl tube + * @param total_pressure pressure inside the pitot/prandtl tube * @param static_pressure pressure at the side of the tube/airplane * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s From 6f1d7dc7de3609f73367794acc8d625c1ca27d03 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Feb 2013 15:48:16 +0100 Subject: [PATCH 30/65] commander app sets an airspeed_valid flag in the vehicle status --- apps/commander/commander.c | 24 +++++++++++++++++++++++- apps/uORB/topics/vehicle_status.h | 1 + 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 41f4e56742..338272b175 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1475,6 +1475,11 @@ int commander_thread_main(int argc, char *argv[]) struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + memset(&differential_pressure, 0, sizeof(differential_pressure)); + uint64_t last_differential_pressure_time = 0; + /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); struct vehicle_command_s cmd; @@ -1528,6 +1533,13 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); } + orb_check(differential_pressure_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure); + last_differential_pressure_time = differential_pressure.timestamp; + } + orb_check(cmd_sub, &new_data); if (new_data) { @@ -1714,6 +1726,7 @@ int commander_thread_main(int argc, char *argv[]) bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; bool global_pos_valid = current_status.flag_global_position_valid; bool local_pos_valid = current_status.flag_local_position_valid; + bool airspeed_valid = current_status.flag_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { @@ -1732,6 +1745,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_local_position_valid = false; } + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_differential_pressure_time < 2000000) { + current_status.flag_airspeed_valid = true; + + } else { + current_status.flag_airspeed_valid = false; + } + /* * Consolidate global position and local position valid flags * for vector flight mode. @@ -1747,7 +1768,8 @@ int commander_thread_main(int argc, char *argv[]) /* consolidate state change, flag as changed if required */ if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid) { + local_pos_valid != current_status.flag_local_position_valid || + airspeed_valid != current_status.flag_airspeed_valid) { state_changed = true; } diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index ae18ba050e..6326f13e6b 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -210,6 +210,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_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ }; /** From de77cba5387018007f0a2b27aeb6984ee9ebc1d0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Feb 2013 11:18:13 +0100 Subject: [PATCH 31/65] small cleanup --- apps/sensors/sensors.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index f77bc9b8b6..6a4cf0c65d 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1041,10 +1041,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f) { -// float pres_raw = fabsf(voltage - (3.3f / 2.0f)); -// float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f; - //XXX depends on sensor used..., where are the above numbers from? - float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP + float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f, _barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa @@ -1074,7 +1071,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } _last_adc = hrt_absolute_time(); - //break; } } } From 8e3b09bd50fed9280328ecdaf2dbc7bd789bbbf9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Feb 2013 13:38:48 +0100 Subject: [PATCH 32/65] small printf change --- apps/systemlib/airspeed.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index d7096d597d..264287b10f 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -97,7 +97,7 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp float density = get_air_density(static_pressure, temperature_celsius); if (density < 0.0001f || !isfinite(density)) { density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; - printf("Invalid air density, using density at sea level\n"); + printf("[airspeed] Invalid air density, using density at sea level\n"); } float pressure_difference = total_pressure - static_pressure; From dee0a30e16e3d4d9725eb16f7205cb1dfa1063cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Feb 2013 21:27:33 +0100 Subject: [PATCH 33/65] Hotfix: ensure PWM output on IO and FMU stops when disarming --- apps/drivers/stm32/drv_pwm_servo.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 954b458f59..2b8641f002 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -299,8 +299,12 @@ up_pwm_servo_arm(bool armed) rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; } else { - /* on disarm, just stop auto-reload so we don't generate runts */ - rCR1(i) &= ~GTIM_CR1_ARPE; + // XXX This leads to FMU PWM being still active + // but uncontrollable. Just disable the timer + // and risk a runt. + ///* on disarm, just stop auto-reload so we don't generate runts */ + //rCR1(i) &= ~GTIM_CR1_ARPE; + rCR1(i) = 0; } } } From 81594c1abc3b7d005e944c19953dca302644a224 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Mar 2013 09:42:39 +0100 Subject: [PATCH 34/65] Hotfix: return correct value for orb_check() for never-published topics --- apps/uORB/uORB.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 532e54b8e0..41f399f2c2 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -429,6 +429,10 @@ ORBDevNode::appears_updated(SubscriberData *sd) /* avoid racing between interrupt and non-interrupt context calls */ irqstate_t state = irqsave(); + /* check if this topic has been published yet, if not bail out */ + if (_data == nullptr) + ret = false; + /* * If the subscriber's generation count matches the update generation * count, there has been no update from their perspective; if they From 433c9548589ebdc1ed40ab9f478b00f531b7bb47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Mar 2013 16:06:52 +0100 Subject: [PATCH 35/65] Hotfix: Fix program flow for uORB non-published topics --- apps/uORB/uORB.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 41f399f2c2..7abbf42aed 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -430,8 +430,10 @@ ORBDevNode::appears_updated(SubscriberData *sd) irqstate_t state = irqsave(); /* check if this topic has been published yet, if not bail out */ - if (_data == nullptr) + if (_data == nullptr) { ret = false; + goto out; + } /* * If the subscriber's generation count matches the update generation @@ -489,6 +491,7 @@ ORBDevNode::appears_updated(SubscriberData *sd) break; } +out: irqrestore(state); /* consider it updated */ From 6eca4ba462440c8b3d45a2e44ac1f2085554d638 Mon Sep 17 00:00:00 2001 From: Greg Hulands Date: Fri, 1 Mar 2013 09:20:00 -0800 Subject: [PATCH 36/65] Maxbotix I2C Sonar Support --- apps/drivers/device/i2c.cpp | 24 + apps/drivers/device/i2c.h | 20 + apps/drivers/drv_range_finder.h | 81 +++ apps/drivers/mb12xx/Makefile | 42 ++ apps/drivers/mb12xx/mb12xx.cpp | 842 +++++++++++++++++++++++++++++ apps/uORB/objects_common.cpp | 3 + apps/uORB/topics/subsystem_info.h | 3 +- nuttx/configs/px4fmu/nsh/appconfig | 1 + 8 files changed, 1015 insertions(+), 1 deletion(-) create mode 100644 apps/drivers/drv_range_finder.h create mode 100644 apps/drivers/mb12xx/Makefile create mode 100644 apps/drivers/mb12xx/mb12xx.cpp diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index a416801eb7..c6102a6c11 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -114,6 +114,30 @@ I2C::probe() return OK; } +int +I2C::write(const uint8_t *send, unsigned send_len) +{ + int ret = OK; + + I2C_SETFREQUENCY(_dev, _frequency); + I2C_SETADDRESS(_dev, _address, 7); + ret = I2C_WRITE(_dev, send, send_len); + + return ret; +} + +int +I2C::read(uint8_t *recv, unsigned recv_len) +{ + int ret = OK; + + I2C_SETFREQUENCY(_dev, _frequency); + I2C_SETADDRESS(_dev, _address, 7); + ret = I2C_READ(_dev, recv, recv_len); + + return ret; +} + int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 66c34dd7c4..874b22301b 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -85,6 +85,26 @@ protected: */ virtual int probe(); + /** + * Perform an I2C write to the device. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int write(const uint8_t *send, unsigned send_len); + + /** + * Perform an I2C read from the device. + * + * @param send Pointer to bytes to send. + * @param send_len Number of bytes to send. + * @return OK if the transfer was successful, -errno + * otherwise. + */ + int read(uint8_t *recv, unsigned recv_len); + /** * Perform an I2C transaction to the device. * diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h new file mode 100644 index 0000000000..420011be5a --- /dev/null +++ b/apps/drivers/drv_range_finder.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * 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 Rangefinder driver interface. + */ + +#ifndef _DRV_RANGEFINDER_H +#define _DRV_RANGEFINDER_H + +#include +#include + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" + +/** + * accel report structure. Reads from the device must be in multiples of this + * structure. + */ +struct range_finder_report { + uint64_t timestamp; + float distance; /** in meters */ + uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ +}; + +/* + * ObjDev tag for raw accelerometer data. + */ +ORB_DECLARE(sensor_range_finder); + +/* + * ioctl() definitions + * + * Accelerometer drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _RANGEFINDERIOCBASE (0x7900) +#define __RANGEFINDERIOC(_n) (_IOC(_RANGEFINDERIOCBASE, _n)) + +/** set the minimum effective distance of the device */ +#define RANGEFINDERIOCSETMINIUMDISTANCE __RANGEFINDERIOC(1) + +/** set the maximum effective distance of the device */ +#define RANGEFINDERIOCSETMAXIUMDISTANCE __RANGEFINDERIOC(2) + + +#endif /* _DRV_RANGEFINDER_H */ diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile new file mode 100644 index 0000000000..b5b911be19 --- /dev/null +++ b/apps/drivers/mb12xx/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 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 +# 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. +# +############################################################################ + +# +# Makefile to build the Maxbotix Sonar driver. +# + +APPNAME = mb12xx +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp new file mode 100644 index 0000000000..d7a3357c80 --- /dev/null +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -0,0 +1,842 @@ +/**************************************************************************** + * + * Copyright (C) 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 + * 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 mb12xx.cpp + * @author Greg Hulands + * + * Driver for the Maxbotix sonar range finders connected via I2C. + */ + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include + +#include +#include + +#include +#include + +/* Configuration Constants */ +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ + +/* MB12xx Registers addresses */ + +#define MB12XX_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define MB12XX_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class MB12XX : public device::I2C +{ +public: + MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); + ~MB12XX(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + range_finder_report *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @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(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be bought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE + * and MB12XX_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); + +MB12XX::MB12XX(int bus, int address) : + I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), + _min_distance(MB12XX_MIN_DISTANCE), + _max_distance(MB12XX_MAX_DISTANCE), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), + _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +MB12XX::~MB12XX() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +MB12XX::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct range_finder_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the range finder topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + + if (_range_finder_topic < 0) + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +MB12XX::probe() +{ + // TODO: take a range reading and see if it is between the min and max + + return OK; +} + +void +MB12XX::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +MB12XX::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +MB12XX::get_minimum_distance() +{ + return _min_distance; +} + +float +MB12XX::get_maximum_distance() +{ + return _max_distance; +} + +int +MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct range_finder_report *buf = new struct range_finder_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: + { + set_minimum_distance((float)arg); + return 0; + } + break; + case RANGEFINDERIOCSETMAXIUMDISTANCE: + { + set_maximum_distance((float)arg); + return 0; + } + break; + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +MB12XX::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _oldest_report = _next_report = 0; + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(MB12XX_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +MB12XX::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd[] = {MB12XX_TAKE_RANGE_REG}; + ret = I2C::write(&cmd[0], 1); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + ret = OK; + + return ret; +} + +int +MB12XX::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + ret = I2C::read(&val[0], 2); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].distance = si_units; + _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; + + return ret; +} + +void +MB12XX::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +MB12XX::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +MB12XX::cycle_trampoline(void *arg) +{ + MB12XX *dev = (MB12XX *)arg; + + dev->cycle(); +} + +void +MB12XX::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + USEC2TICK(MB12XX_CONVERSION_INTERVAL)); +} + +void +MB12XX::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace mb12xx +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MB12XX *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new MB12XX(MB12XX_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +mb12xx_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + mb12xx::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + mb12xx::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + mb12xx::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + mb12xx::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + mb12xx::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 3f3476f62a..1363751401 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -53,6 +53,9 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include ORB_DEFINE(sensor_baro, struct baro_report); +#include +ORB_DEFINE(sensor_range_finder, struct range_finder_report); + #include ORB_DEFINE(output_pwm, struct pwm_output_values); diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h index c3e039d66b..c415e832e0 100644 --- a/apps/uORB/topics/subsystem_info.h +++ b/apps/uORB/topics/subsystem_info.h @@ -71,7 +71,8 @@ enum SUBSYSTEM_TYPE SUBSYSTEM_TYPE_YAWPOSITION = 4096, SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384, SUBSYSTEM_TYPE_POSITIONCONTROL = 32768, - SUBSYSTEM_TYPE_MOTORCONTROL = 65536 + SUBSYSTEM_TYPE_MOTORCONTROL = 65536, + SUBSYSTEM_TYPE_RANGEFINDER = 131072 }; /** diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 2bf3283c81..f1f70e2283 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -125,6 +125,7 @@ CONFIGURED_APPS += drivers/stm32/adc CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil CONFIGURED_APPS += drivers/gps +CONFIGURED_APPS += drivers/mb12xx # Testing stuff CONFIGURED_APPS += px4/sensors_bringup From 349af372d0823d3e7f8cbc0c247d2af81e7c44a9 Mon Sep 17 00:00:00 2001 From: Greg Hulands Date: Fri, 1 Mar 2013 10:03:40 -0800 Subject: [PATCH 37/65] Changes from pull request feedback --- apps/drivers/device/i2c.cpp | 24 ------------------------ apps/drivers/device/i2c.h | 20 -------------------- apps/drivers/drv_range_finder.h | 4 ++-- apps/drivers/mb12xx/Makefile | 2 +- apps/drivers/mb12xx/mb12xx.cpp | 12 ++++++------ 5 files changed, 9 insertions(+), 53 deletions(-) diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp index c6102a6c11..a416801eb7 100644 --- a/apps/drivers/device/i2c.cpp +++ b/apps/drivers/device/i2c.cpp @@ -114,30 +114,6 @@ I2C::probe() return OK; } -int -I2C::write(const uint8_t *send, unsigned send_len) -{ - int ret = OK; - - I2C_SETFREQUENCY(_dev, _frequency); - I2C_SETADDRESS(_dev, _address, 7); - ret = I2C_WRITE(_dev, send, send_len); - - return ret; -} - -int -I2C::read(uint8_t *recv, unsigned recv_len) -{ - int ret = OK; - - I2C_SETFREQUENCY(_dev, _frequency); - I2C_SETADDRESS(_dev, _address, 7); - ret = I2C_READ(_dev, recv, recv_len); - - return ret; -} - int I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len) { diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 874b22301b..472bf26abd 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -84,26 +84,6 @@ protected: * Check for the presence of the device on the bus. */ virtual int probe(); - - /** - * Perform an I2C write to the device. - * - * @param send Pointer to bytes to send. - * @param send_len Number of bytes to send. - * @return OK if the transfer was successful, -errno - * otherwise. - */ - int write(const uint8_t *send, unsigned send_len); - - /** - * Perform an I2C read from the device. - * - * @param send Pointer to bytes to send. - * @param send_len Number of bytes to send. - * @return OK if the transfer was successful, -errno - * otherwise. - */ - int read(uint8_t *recv, unsigned recv_len); /** * Perform an I2C transaction to the device. diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h index 420011be5a..0fe8564acf 100644 --- a/apps/drivers/drv_range_finder.h +++ b/apps/drivers/drv_range_finder.h @@ -57,14 +57,14 @@ struct range_finder_report { }; /* - * ObjDev tag for raw accelerometer data. + * ObjDev tag for raw range finder data. */ ORB_DECLARE(sensor_range_finder); /* * ioctl() definitions * - * Accelerometer drivers also implement the generic sensor driver + * Rangefinder drivers also implement the generic sensor driver * interfaces from drv_sensor.h */ diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile index b5b911be19..0d24057877 100644 --- a/apps/drivers/mb12xx/Makefile +++ b/apps/drivers/mb12xx/Makefile @@ -37,6 +37,6 @@ APPNAME = mb12xx PRIORITY = SCHED_PRIORITY_DEFAULT -STACKSIZE = 4096 +STACKSIZE = 2048 include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp index d7a3357c80..0e4712f5a9 100644 --- a/apps/drivers/mb12xx/mb12xx.cpp +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -157,7 +157,7 @@ private: /** * Set the min and max distance thresholds if you want the end points of the sensors - * range to be bought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE + * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE * and MB12XX_MAX_DISTANCE */ void set_minimum_distance(float min); @@ -388,13 +388,13 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) case RANGEFINDERIOCSETMINIUMDISTANCE: { - set_minimum_distance((float)arg); + set_minimum_distance(*(float *)arg); return 0; } break; case RANGEFINDERIOCSETMAXIUMDISTANCE: { - set_maximum_distance((float)arg); + set_maximum_distance(*(float *)arg); return 0; } break; @@ -471,8 +471,8 @@ MB12XX::measure() /* * Send the command to begin a measurement. */ - uint8_t cmd[] = {MB12XX_TAKE_RANGE_REG}; - ret = I2C::write(&cmd[0], 1); + uint8_t cmd = MB12XX_TAKE_RANGE_REG; + ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { @@ -495,7 +495,7 @@ MB12XX::collect() perf_begin(_sample_perf); - ret = I2C::read(&val[0], 2); + ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { From d1e41f2c48a4ccf21634abe858454889778f7a32 Mon Sep 17 00:00:00 2001 From: Greg Hulands Date: Fri, 1 Mar 2013 10:14:11 -0800 Subject: [PATCH 38/65] Missed the accel reference here --- apps/drivers/drv_range_finder.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h index 0fe8564acf..03a82ec5d0 100644 --- a/apps/drivers/drv_range_finder.h +++ b/apps/drivers/drv_range_finder.h @@ -47,7 +47,7 @@ #define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" /** - * accel report structure. Reads from the device must be in multiples of this + * range finder report structure. Reads from the device must be in multiples of this * structure. */ struct range_finder_report { From 160ac722bedaad0e8cb824b14f902785976e1d2b Mon Sep 17 00:00:00 2001 From: Greg Hulands Date: Fri, 1 Mar 2013 10:16:04 -0800 Subject: [PATCH 39/65] Fix white space --- apps/drivers/device/i2c.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 472bf26abd..66c34dd7c4 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -84,7 +84,7 @@ protected: * Check for the presence of the device on the bus. */ virtual int probe(); - + /** * Perform an I2C transaction to the device. * From 42694a5736f0b4493318f0d1c03cab70752ddec7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 2 Mar 2013 22:17:43 -0800 Subject: [PATCH 40/65] Make some improvements to the ARMv7M fault decode logic. --- Debug/ARMv7M | 204 +++++++++++++++++++++++++++------------------------ 1 file changed, 108 insertions(+), 96 deletions(-) diff --git a/Debug/ARMv7M b/Debug/ARMv7M index 3d96da682d..803d964534 100644 --- a/Debug/ARMv7M +++ b/Debug/ARMv7M @@ -19,120 +19,132 @@ end define vecstate - set $icsr = *(uint32_t *)0xe000ed04 + set $icsr = *(unsigned *)0xe000ed04 set $vect = $icsr & 0x1ff set $pend = ($icsr & 0x1ff000) >> 12 - set $shcsr = *(uint32_t *)0xe000ed24 - set $cfsr = *(uint32_t *)0xe000ed28 + set $shcsr = *(unsigned *)0xe000ed24 + set $cfsr = *(unsigned *)0xe000ed28 set $mmfsr = $cfsr & 0xff set $bfsr = ($cfsr >> 8) & 0xff set $ufsr = ($cfsr >> 16) & 0xffff - set $hfsr = *(uint32_t *)0xe000ed2c - set $bfar = *(uint32_t *)0xe000ed38 - set $mmfar = *(uint32_t *)0xe000ed34 + set $hfsr = *(unsigned *)0xe000ed2c + set $bfar = *(unsigned *)0xe000ed38 + set $mmfar = *(unsigned *)0xe000ed34 - # XXX Currently, rather than look at $vect, we just decode the - # fault status registers directly. + if $vect < 15 - if $hfsr != 0 - printf "HardFault:" - if $hfsr & (1<<1) - printf " due to vector table read fault\n" + if $hfsr != 0 + printf "HardFault:" + if $hfsr & (1<<1) + printf " due to vector table read fault\n" + end + if $hfsr & (1<<30) + printf " forced due to escalated or disabled configurable fault (see below)\n" + end + if $hfsr & (1<<31) + printf " due to an unexpected debug event\n" + end end - if $hfsr & (1<<30) - printf " forced ue to escalated or disabled configurable fault (see below)\n" + if $mmfsr != 0 + printf "MemManage:" + if $mmfsr & (1<<5) + printf " during lazy FP state save" + end + if $mmfsr & (1<<4) + printf " during exception entry" + end + if $mmfsr & (1<<3) + printf " during exception return" + end + if $mmfsr & (1<<0) + printf " during data access" + end + if $mmfsr & (1<<0) + printf " during instruction prefetch" + end + if $mmfsr & (1<<7) + printf " accessing 0x%08x", $mmfar + end + printf "\n" end - if $hfsr & (1<<31) - printf " due to an unexpected debug event\n" + if $bfsr != 0 + printf "BusFault:" + if $bfsr & (1<<2) + printf " (imprecise)" + end + if $bfsr & (1<<1) + printf " (precise)" + end + if $bfsr & (1<<5) + printf " during lazy FP state save" + end + if $bfsr & (1<<4) + printf " during exception entry" + end + if $bfsr & (1<<3) + printf " during exception return" + end + if $bfsr & (1<<0) + printf " during instruction prefetch" + end + if $bfsr & (1<<7) + printf " accessing 0x%08x", $bfar + end + printf "\n" + end + if $ufsr != 0 + printf "UsageFault" + if $ufsr & (1<<9) + printf " due to divide-by-zero" + end + if $ufsr & (1<<8) + printf " due to unaligned memory access" + end + if $ufsr & (1<<3) + printf " due to access to disabled/absent coprocessor" + end + if $ufsr & (1<<2) + printf " due to a bad EXC_RETURN value" + end + if $ufsr & (1<<1) + printf " due to bad T or IT bits in EPSR" + end + if $ufsr & (1<<0) + printf " due to executing an undefined instruction" + end + printf "\n" + end + else + if $vect >= 15 + printf "Handling vector %u\n", $vect end end - if $mmfsr != 0 - printf "MemManage:" - if $mmfsr & (1<<5) - printf " during lazy FP state save" - end - if $mmfsr & (1<<4) - printf " during exception entry" - end - if $mmfsr & (1<<3) - printf " during exception return" - end - if $mmfsr & (1<<0) - printf " during data access" - end - if $mmfsr & (1<<0) - printf " during instruction prefetch" - end - if $mmfsr & (1<<7) - printf " accessing 0x%08x", $mmfar - end - printf "\n" - end - if $bfsr != 0 - printf "BusFault:" - if $bfsr & (1<<2) - printf " (imprecise)" - end - if $bfsr & (1<<1) - printf " (precise)" - end - if $bfsr & (1<<5) - printf " during lazy FP state save" - end - if $bfsr & (1<<4) - printf " during exception entry" - end - if $bfsr & (1<<3) - printf " during exception return" - end - if $bfsr & (1<<0) - printf " during instruction prefetch" - end - if $bfsr & (1<<7) - printf " accessing 0x%08x", $bfar - end - printf "\n" - end - if $ufsr != 0 - printf "UsageFault" - if $ufsr & (1<<9) - printf " due to divide-by-zero" - end - if $ufsr & (1<<8) - printf " due to unaligned memory access" - end - if $ufsr & (1<<3) - printf " due to access to disabled/absent coprocessor" - end - if $ufsr & (1<<2) - printf " due to a bad EXC_RETURN value" - end - if $ufsr & (1<<1) - printf " due to bad T or IT bits in EPSR" - end - if $ufsr & (1<<0) - printf " due to executing an undefined instruction" - end - printf "\n" - end - if ((uint32_t)$lr & 0xf0000000) == 0xf0000000 + if ((unsigned)$lr & 0xf0000000) == 0xf0000000 if ($lr & 1) - set $frame_ptr = (uint32_t *)$msp + printf "exception frame is on MSP\n" + #set $frame_ptr = (unsigned *)$msp + set $frame_ptr = (unsigned *)$sp else - set $frame_ptr = (uint32_t *)$psp + printf "exception frame is on PSP, backtrace may not be possible\n" + #set $frame_ptr = (unsigned *)$psp + set $frame_ptr = (unsigned *)$sp end - printf " r0: %08x r1: %08x r2: %08x r3: %08x\n, $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3] + if $lr & 0x10 + set $fault_sp = $frame_ptr + (8 * 4) + else + set $fault_sp = $frame_ptr + (26 * 4) + end + + + printf " r0: %08x r1: %08x r2: %08x r3: %08x\n", $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3] printf " r4: %08x r5: %08x r6: %08x r7: %08x\n", $r4, $r5, $r6, $r7 printf " r8: %08x r9: %08x r10: %08x r11: %08x\n", $r8, $r9, $r10, $r11 - printf " r12: $08x lr: %08x pc: %08xx PSR: %08x\n", $frame_ptr[4], $frame_ptr[5], $frame_ptr[6], $frame_ptr[7] + printf " r12: %08x\n", $frame_ptr[4] + printf " sp: %08x lr: %08x pc: %08x PSR: %08x\n", $fault_sp, $frame_ptr[5], $frame_ptr[6], $frame_ptr[7] # Swap to the context of the faulting code and try to print a backtrace set $saved_sp = $sp - if $lr & 0x10 - set $sp = $frame_ptr + (8 * 4) - else - set $sp = $frame_ptr + (26 * 4) - end + set $sp = $fault_sp set $saved_lr = $lr set $lr = $frame_ptr[5] set $saved_pc = $pc @@ -142,7 +154,7 @@ define vecstate set $lr = $saved_lr set $pc = $saved_pc else - printf "(not currently in exception state)\n" + printf "(not currently in exception handler)\n" end end From e27481826c72d71c28740b3ecfa3ea9471b8687d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 3 Mar 2013 23:43:17 +0100 Subject: [PATCH 41/65] write adc values to sensors combined and log them --- ROMFS/logging/logconv.m | 4 ++-- apps/sdlog/sdlog.c | 2 +- apps/sdlog/sdlog_ringbuffer.h | 4 ++-- apps/sensors/sensors.cpp | 4 ++++ 4 files changed, 9 insertions(+), 5 deletions(-) diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m index 5ea2aeb95a..92ee014135 100755 --- a/ROMFS/logging/logconv.m +++ b/ROMFS/logging/logconv.m @@ -33,7 +33,7 @@ end % float vbat; //battery voltage in [volt] % float bat_current - current drawn from battery at this time instant % float bat_discharged - discharged energy in mAh -% float adc[3]; //remaining auxiliary ADC ports [volt] +% float adc[4]; //ADC ports [volt] % float local_position[3]; //tangent plane mapping into x,y,z [m] % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] % float attitude[3]; //pitch, roll, yaw [rad] @@ -57,7 +57,7 @@ logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, ' logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 4fd5fea1b9..df8745d9f5 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -683,7 +683,7 @@ int sdlog_thread_main(int argc, char *argv[]) .vbat = buf.batt.voltage_v, .bat_current = buf.batt.current_a, .bat_discharged = buf.batt.discharged_mah, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, + .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]}, .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, diff --git a/apps/sdlog/sdlog_ringbuffer.h b/apps/sdlog/sdlog_ringbuffer.h index f7fc0d4509..b65916459e 100644 --- a/apps/sdlog/sdlog_ringbuffer.h +++ b/apps/sdlog/sdlog_ringbuffer.h @@ -56,7 +56,7 @@ struct sdlog_sysvector { float vbat; /**< battery voltage in [volt] */ float bat_current; /**< battery discharge current */ float bat_discharged; /**< discharged energy in mAh */ - float adc[3]; /**< remaining auxiliary ADC ports [volt] */ + float adc[4]; /**< ADC ports [volt] */ float local_position[3]; /**< tangent plane mapping into x,y,z [m] */ int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */ float attitude[3]; /**< roll, pitch, yaw [rad] */ @@ -88,4 +88,4 @@ void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvec int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem); -#endif \ No newline at end of file +#endif diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 6a4cf0c65d..b5badec711 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1004,6 +1004,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (ret >= (int)sizeof(buf_adc[0])) { + if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { + raw.adc_voltage_v[i] = buf_adc[i].am_data; + } + if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* Voltage in volts */ float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); From c149361f1535eccb8f077052fb67c5cd56db7169 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 4 Mar 2013 14:21:49 +0100 Subject: [PATCH 42/65] scale the saved adc values to get voltage --- apps/sensors/sensors.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index b5badec711..becd32f7d1 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -998,16 +998,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); - /* look for battery channel */ - for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { if (ret >= (int)sizeof(buf_adc[0])) { + /* Save raw voltage values */ if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { - raw.adc_voltage_v[i] = buf_adc[i].am_data; + raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); } + /* look for specific channels and process the raw voltage to measurement data */ if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* Voltage in volts */ float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); From b526bab1748f8a74202caf42966a5a719bba28ad Mon Sep 17 00:00:00 2001 From: px4dev Date: Mon, 4 Mar 2013 21:46:55 -0800 Subject: [PATCH 43/65] Remove extra spaces from mixers before processing them. This gives us some more working space on IO for mixer processing. --- apps/systemcmds/mixer/mixer.c | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index e2f7b5bd58..55c4f08362 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -117,7 +117,23 @@ load(const char *devname, const char *fname) if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) continue; - /* XXX an optimisation here would be to strip extra whitespace */ + /* compact whitespace in the buffer */ + char *t, *f; + for (f = buf; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) From 16aa618bb4338899fc5e4c239efc741818d725db Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 15 Dec 2012 08:38:13 +0100 Subject: [PATCH 44/65] testing --- apps/drivers/px4fmu/fmu.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 430d18c6d5..444f28b374 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -297,6 +297,7 @@ PX4FMU::set_pwm_rate(unsigned rate) if ((rate > 500) || (rate < 10)) return -EINVAL; +<<<<<<< HEAD:apps/drivers/px4fmu/fmu.cpp _update_rate = rate; return OK; } @@ -312,6 +313,11 @@ PX4FMU::task_main() ORB_ID(actuator_controls_1)); /* force a reset of the update rate */ _current_update_rate = 0; +======= + /* subscribe to objects that we are interested in watching */ + _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); + orb_set_interval(_t_actuators, 5); /* 200Hz update rate */ +>>>>>>> testing:apps/px4/fmu/fmu.cpp _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ From 07c349382555ff06d749a62e499c34feb8386b76 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 5 Mar 2013 23:00:39 +0100 Subject: [PATCH 45/65] Revert "testing" This reverts commit 16aa618bb4338899fc5e4c239efc741818d725db. --- apps/drivers/px4fmu/fmu.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index 444f28b374..430d18c6d5 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -297,7 +297,6 @@ PX4FMU::set_pwm_rate(unsigned rate) if ((rate > 500) || (rate < 10)) return -EINVAL; -<<<<<<< HEAD:apps/drivers/px4fmu/fmu.cpp _update_rate = rate; return OK; } @@ -313,11 +312,6 @@ PX4FMU::task_main() ORB_ID(actuator_controls_1)); /* force a reset of the update rate */ _current_update_rate = 0; -======= - /* subscribe to objects that we are interested in watching */ - _t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - orb_set_interval(_t_actuators, 5); /* 200Hz update rate */ ->>>>>>> testing:apps/px4/fmu/fmu.cpp _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 200); /* 5Hz update rate */ From ae98836db8948edbcf59333627b25f69df4127d4 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 6 Mar 2013 20:37:01 +0100 Subject: [PATCH 46/65] Correct RC config sanity checking and report back when RC config errors occur. --- apps/px4io/mixer.cpp | 8 +++--- apps/px4io/protocol.h | 1 + apps/px4io/px4io.c | 3 +++ apps/px4io/registers.c | 57 +++++++++++++++++++++++++++++------------- apps/px4io/safety.c | 7 +++++- 5 files changed, 55 insertions(+), 21 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ed74cb3d30..0fba2cbe55 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -170,9 +170,11 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - 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))); + 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)) && + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 0ee6d2c4b4..14d221b5b5 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -103,6 +103,7 @@ #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_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #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 */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 5892646612..a0e0002a64 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -153,6 +153,9 @@ user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); + /* initialise the registry space */ + registers_init(); + /* initialise the control inputs */ controls_init(); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 5ec2f7258b..dac09021db 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -179,6 +179,12 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; +void +registers_init(void) +{ + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; +} + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -390,27 +396,44 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* should the channel be enabled? */ /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + /* 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; + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + /* 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; + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + // The following check isn't needed as constraint checks in controls.c will catch this. + //if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + //if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + count++; + } /* sanity checks pass, enable channel */ - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + } else { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } } break; /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 540636e19d..5495d5ae12 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -176,12 +176,17 @@ heartbeat_blink(void *arg) static void failsafe_blink(void *arg) { + /* indicate that a serious initialisation error occured */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + LED_AMBER(true); + return; + } + static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; - } else { failsafe = false; } From 5c12b6a91113e924f2264e1b0d04d6f865eb3c64 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Wed, 6 Mar 2013 22:52:19 +0100 Subject: [PATCH 47/65] Request result of rc config upload from IO --- apps/drivers/px4io/px4io.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 2611c4e9c7..fee49b1aa0 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -83,6 +83,7 @@ #include #include "uploader.h" +#include class PX4IO : public device::I2C @@ -771,9 +772,17 @@ PX4IO::io_set_rc_config() /* 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"); + log("rc config upload failed"); break; } + + /* check the IO initialisation flag */ + ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK; + if (ret != OK) { + log("config for RC%d rejected by IO", i + 1); + break; + } + offset += PX4IO_P_RC_CONFIG_STRIDE; } From 8d1f80a9e8ef988949eed006995384800ac91e70 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 7 Mar 2013 01:03:38 +0100 Subject: [PATCH 48/65] Fix how we check for rc config init status --- apps/drivers/px4io/px4io.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index fee49b1aa0..8fb53295f4 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -777,8 +777,7 @@ PX4IO::io_set_rc_config() } /* check the IO initialisation flag */ - ret = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK; - if (ret != OK) { + if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { log("config for RC%d rejected by IO", i + 1); break; } @@ -1195,7 +1194,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); - printf("status 0x%04x%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\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), @@ -1206,7 +1205,8 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), - ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC")); + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s\n", alarms, From 8f5dac3740c87636f1f000b7e67df6f8ad58822a Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 7 Mar 2013 01:47:02 +0100 Subject: [PATCH 49/65] Let's just init the status flag every time we send a config update --- apps/px4io/px4io.c | 3 --- apps/px4io/registers.c | 7 +------ 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index a0e0002a64..5892646612 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -153,9 +153,6 @@ user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); - /* initialise the registry space */ - registers_init(); - /* initialise the control inputs */ controls_init(); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index dac09021db..d97fd8d86e 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -179,12 +179,6 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT]; -void -registers_init(void) -{ - r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; -} - void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -389,6 +383,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_RC_CONFIG_OPTIONS: value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; From 4797c192be73fee2f99769a4063044c97632899e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 09:49:12 +0100 Subject: [PATCH 50/65] Fixed RC calibration scaling / assignment --- apps/px4io/controls.c | 45 +++++++++++++++++++++++++++---------------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index dabdde0af9..908334c478 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -147,32 +147,43 @@ controls_tick() { 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]; - } + int16_t scaled; - /* constrain to min/max values */ + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ 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]; - int16_t scaled = raw; + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if center is min 0..20000, if center is max -20000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 20.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); - /* adjust to zero-relative around center (nominal -500..500) */ - scaled -= conf[PX4IO_P_RC_CONFIG_CENTER]; + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 20.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); - /* scale to fixed-point representation (-10000..10000) */ - scaled *= 20; + } else { + /* in the configured dead zone, output zero */ + scaled = 0; + } + /* invert channel if requested */ if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) scaled = -scaled; From c993ba5bbc9e9a2781d26a5837b5711298de45ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 10:27:55 +0100 Subject: [PATCH 51/65] Fixed minor scaling issue, throttle range still half --- apps/px4io/controls.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 908334c478..b3b6398579 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -173,10 +173,10 @@ controls_tick() { * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 20.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + scaled = 20000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 20.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + scaled = 20000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); } else { /* in the configured dead zone, output zero */ From ff5ca82c7546d5e6db69144b57fb2878c4585ddf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 11:45:23 +0100 Subject: [PATCH 52/65] Fixed throttle scaling issue, harmonized FMU and IO RC scaling code --- apps/px4io/controls.c | 7 ++++--- apps/sensors/sensors.cpp | 42 ++++++++++++++++++++++++++-------------- 2 files changed, 32 insertions(+), 17 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index b3b6398579..d678fd351e 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -164,7 +164,8 @@ controls_tick() { * * First normalize to 0..1 range with correct sign (below or above center), * then scale to 20000 range (if center is an actual center, -10000..10000, - * if center is min 0..20000, if center is max -20000..0). + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). * * As the min and max bounds were enforced in step 1), division by zero * cannot occur, as for the case of center == min or center == max the if @@ -173,10 +174,10 @@ controls_tick() { * DO NOT REMOVE OR ALTER STEP 1! */ if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 20000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { - scaled = 20000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); } else { /* in the configured dead zone, output zero */ diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 9b95c09397..8d04e6ad68 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1125,31 +1125,45 @@ Sensors::ppm_poll() /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { - /* scale around the mid point differently for lower and upper range */ + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (rc_input.values[i] < _parameters.min[i]) + rc_input.values[i] = _parameters.min[i]; + if (rc_input.values[i] > _parameters.max[i]) + rc_input.values[i] = _parameters.max[i]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * the total range is 2 (-1..1). + * If center (trim) == min, scale to 0..1, if center (trim) == max, + * scale to -1..0. + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ _rc.chan[i].scaled = 0.0f; } - /* reverse channel if required */ - if (i == (int)_rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } - - } else { - _rc.chan[i].scaled *= _parameters.rev[i]; - } + _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + if (!isfinite(_rc.chan[i].scaled)) _rc.chan[i].scaled = 0.0f; } From a49382485094245b52ded5c3799a9bdd6b0fa832 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 18:06:20 +0100 Subject: [PATCH 53/65] Fixed wrong comment --- apps/px4io/controls.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index d678fd351e..2d99116f80 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -268,10 +268,11 @@ controls_tick() { bool override = false; /* - * Check mapped channel 5; if the value is 'high' then the pilot has + * Check mapped channel 5 (can be any remote channel, + * depends on RC_MAP_OVER parameter); + * 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; From ebac51cad8e144b64938e6726e26bdc23aaf45e5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 19:47:43 +0100 Subject: [PATCH 54/65] Working on restart resilience, hunting down multi-load mixer issue (still present) --- apps/drivers/px4io/px4io.cpp | 38 ++++++++++++++++++++-------- apps/px4io/mixer.cpp | 16 +++++++++--- apps/px4io/registers.c | 7 +++++ apps/systemlib/mixer/mixer.cpp | 1 + apps/systemlib/mixer/mixer_group.cpp | 1 + 5 files changed, 49 insertions(+), 14 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 8fb53295f4..882ca9fed8 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1556,7 +1556,7 @@ px4io_main(int argc, char *argv[]) errx(1, "already loaded"); /* create the driver - it will set g_dev */ - (void)new PX4IO; + (void)new PX4IO(); if (g_dev == nullptr) errx(1, "driver alloc failed"); @@ -1567,7 +1567,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 && strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { if (argc > 2 + 1) { #warning implement this } else { @@ -1579,16 +1579,31 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "recovery")) { + + if (g_dev != nullptr) { + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } - exit(0); + if (g_dev != nullptr) { + /* stop the driver */ + delete g_dev; + } else { + errx(1, "not loaded"); } + exit(0); + } if (!strcmp(argv[1], "status")) { @@ -1613,8 +1628,9 @@ px4io_main(int argc, char *argv[]) exit(1); } uint8_t level = atoi(argv[2]); - // we can cheat and call the driver directly, as it - // doesn't reference filp in ioctl() + /* 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); diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 0fba2cbe55..54584e6851 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -108,9 +108,11 @@ mixer_tick(void) /* * Decide which set of controls we're using. */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - /* don't actually mix anything - we already have raw PWM values */ + /* don't actually mix anything - we already have raw PWM values or + not a valid mixer. */ source = MIX_NONE; } else { @@ -239,6 +241,11 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { + /* do not allow a mixer change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + return; + } px4io_mixdata *msg = (px4io_mixdata *)buffer; @@ -252,9 +259,12 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); + + /* FIRST mark the mixer as invalid */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index d97fd8d86e..511a47f8df 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -361,6 +361,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_PAGE_RC_CONFIG: { + + /* do not allow a RC config change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + break; + } + 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]; diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 72f765d90c..df0dfe838d 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -54,6 +54,7 @@ #include "mixer.h" Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), _control_cb(control_cb), _cb_handle(cb_handle) { diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 60eeff2256..1dbc512cdb 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -93,6 +93,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; + mixer = nullptr; } } From e8e52afcc426491f5959e6f879f26c9211a88d4c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 7 Mar 2013 20:51:33 +0100 Subject: [PATCH 55/65] Added minimum set of IO MAVLink text messages, report critical errors such as in-air restarts --- apps/drivers/px4io/px4io.cpp | 19 +++++++++++++++++-- apps/mavlink/mavlink_log.h | 12 ++++++++++++ apps/px4io/px4io.c | 8 ++++++-- 3 files changed, 35 insertions(+), 4 deletions(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 882ca9fed8..f9ffa6bcdb 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -82,6 +82,7 @@ #include #include +#include #include "uploader.h" #include @@ -112,6 +113,8 @@ private: volatile int _task; ///< worker task volatile bool _task_should_exit; + int _mavlink_fd; + perf_counter_t _perf_update; /* cached IO state */ @@ -286,6 +289,7 @@ PX4IO::PX4IO() : _update_interval(0), _task(-1), _task_should_exit(false), + _mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), _status(0), _alarms(0), @@ -302,6 +306,9 @@ PX4IO::PX4IO() : /* we need this potentially before it could be set in task_main */ g_dev = this; + /* open MAVLink text channel */ + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + _debug_enabled = true; } @@ -355,6 +362,7 @@ PX4IO::init() (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) @@ -381,6 +389,8 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + /* 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). @@ -470,6 +480,7 @@ PX4IO::init() ret = io_set_rc_config(); if (ret != OK) { log("failed to update RC input config"); + mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); return ret; } @@ -491,6 +502,8 @@ PX4IO::init() return -errno; } + mavlink_log_info(_mavlink_fd, "[IO] init ok"); + return OK; } @@ -1165,9 +1178,11 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) /* check for the mixer-OK flag */ if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { debug("mixer upload OK"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; } else { debug("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); } /* load must have failed for some reason */ @@ -1484,7 +1499,7 @@ test(void) servos[i] = pwm_value; ret = write(fd, servos, sizeof(servos)); - if (ret != sizeof(servos)) + if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); if (direction > 0) { @@ -1567,7 +1582,7 @@ px4io_main(int argc, char *argv[]) } /* look for the optional pwm update rate for the supported modes */ - if (argc > 2 && strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) { if (argc > 2 + 1) { #warning implement this } else { diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index 62e6f7ca07..233a76cb31 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -63,7 +63,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#else #define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#endif /** * Send a mavlink critical message. @@ -71,7 +75,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#else #define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#endif /** * Send a mavlink info message. @@ -79,7 +87,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#else #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#endif struct mavlink_logmessage { char text[51]; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 5892646612..0c48385231 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -215,12 +215,16 @@ user_start(int argc, char *argv[]) /* post debug state at ~1Hz */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", + + struct mallinfo minfo = mallinfo(); + + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", (unsigned)debug_level, (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets); + (unsigned)i2c_loop_resets, + (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } } From cc628fbc27794fee52e3a6f33d091758ca1cb51a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 11:03:06 +0100 Subject: [PATCH 56/65] Add missing mixer ok check in override mode, clear FMU lost alarm when setting FMU_OK flag, print AP RX timeout in production mode as well --- apps/px4io/mixer.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index ed74cb3d30..65de864ec3 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -92,7 +92,7 @@ mixer_tick(void) /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - debug("AP RX timeout"); + lowsyslog("AP RX timeout"); } r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; @@ -101,6 +101,7 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; } source = MIX_FAILSAFE; @@ -123,7 +124,8 @@ mixer_tick(void) } if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; From 11cb9df05b3a0fb3312de59db2cf44238b10517c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 11:20:06 +0100 Subject: [PATCH 57/65] After the mb12xx driver was merged way too early, make the best out of it and fix up the init phase to the driver bails out if there is no sensor connected --- apps/drivers/mb12xx/mb12xx.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp index 0e4712f5a9..406c726fb6 100644 --- a/apps/drivers/mb12xx/mb12xx.cpp +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -259,9 +259,7 @@ out: int MB12XX::probe() { - // TODO: take a range reading and see if it is between the min and max - - return OK; + return measure(); } void From 4b26d7aef4e1b1d714b55cfb7abb48adcfd8c975 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 9 Mar 2013 12:27:29 +0100 Subject: [PATCH 58/65] adding missing include --- apps/px4io/mixer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 092a35a443..ec69cdd64e 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -38,6 +38,7 @@ */ #include +#include #include #include From 74bcf29c698ee9b6f8a7859d59c28f4a69a54e02 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 13:20:05 +0100 Subject: [PATCH 59/65] Refactored debug level into proper register, px4io status now correctly reads it. Added more of the missing alarms clear logic, alarms reporting now consistent. Adding missing sign change on mode switch, fixes override issue when attempting to switch to auto mode. Pending outdoor tests --- apps/px4io/controls.c | 4 +++- apps/px4io/px4io.c | 7 +++---- apps/px4io/registers.c | 8 +++++--- 3 files changed, 11 insertions(+), 8 deletions(-) diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index 2d99116f80..b507078a17 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -213,7 +213,9 @@ controls_tick() { if (assigned_channels == 0) { rc_input_lost = true; } else { + /* set RC OK flag and clear RC lost alarm */ r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST; } /* @@ -274,7 +276,7 @@ controls_tick() { * requested override. * */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) override = true; if (override) { diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 0c48385231..9de37e1188 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -64,8 +64,7 @@ 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; +/* store i2c reset count XXX this should be a register, together with other error counters */ volatile uint32_t i2c_loop_resets = 0; /* @@ -90,7 +89,7 @@ static char msg[NUM_MSG][40]; void isr_debug(uint8_t level, const char *fmt, ...) { - if (level > debug_level) { + if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { return; } va_list ap; @@ -219,7 +218,7 @@ user_start(int argc, char *argv[]) struct mallinfo minfo = mallinfo(); isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", - (unsigned)debug_level, + (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index 511a47f8df..645c1565d7 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -137,7 +137,8 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_RELAYS] = 0, [PX4IO_P_SETUP_VBATT_SCALE] = 10000, [PX4IO_P_SETUP_IBATT_SCALE] = 0, - [PX4IO_P_SETUP_IBATT_BIAS] = 0 + [PX4IO_P_SETUP_IBATT_BIAS] = 0, + [PX4IO_P_SETUP_SET_DEBUG] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) @@ -201,6 +202,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; break; @@ -351,8 +353,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_SET_DEBUG: - debug_level = value; - isr_debug(0, "set debug %u\n", (unsigned)debug_level); + r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; + isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; default: From e7df439ea1f99f26969b6b741a239c722599f194 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 13:21:23 +0100 Subject: [PATCH 60/65] Hotfix: Extend GPS lost timeout by a small delta to prevent timeout aliasing, GPS app does not report any more losses --- apps/drivers/gps/gps.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 6d7cfcc688..1350106539 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -67,7 +67,7 @@ #include "mtk.h" -#define TIMEOUT_5HZ 400 +#define TIMEOUT_5HZ 500 #define RATE_MEASUREMENT_PERIOD 5000000 /* oddly, ERROR is not defined for c++ */ From 802d0ae2faa101b2a9eaef75f4019160faf250fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 9 Mar 2013 21:07:29 +0100 Subject: [PATCH 61/65] Made dtors virtual, tested on IO and FMU --- apps/drivers/blinkm/blinkm.cpp | 2 +- apps/drivers/bma180/bma180.cpp | 2 +- apps/drivers/gps/gps.cpp | 2 +- apps/drivers/hil/hil.cpp | 2 +- apps/drivers/hmc5883/hmc5883.cpp | 2 +- apps/drivers/l3gd20/l3gd20.cpp | 2 +- apps/drivers/led/led.cpp | 2 +- apps/drivers/mb12xx/mb12xx.cpp | 2 +- apps/drivers/mpu6000/mpu6000.cpp | 2 +- apps/drivers/ms5611/ms5611.cpp | 2 +- apps/drivers/px4fmu/fmu.cpp | 2 +- apps/drivers/px4io/px4io.cpp | 2 +- apps/drivers/px4io/uploader.h | 2 +- apps/mavlink_onboard/mavlink.c | 2 +- apps/systemlib/mixer/mixer.h | 2 +- 15 files changed, 15 insertions(+), 15 deletions(-) diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index fc929284c4..54c7d4266a 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -127,7 +127,7 @@ class BlinkM : public device::I2C { public: BlinkM(int bus, int blinkm); - ~BlinkM(); + virtual ~BlinkM(); virtual int init(); diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index 32eb5333ea..4409a8a9cb 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/apps/drivers/bma180/bma180.cpp @@ -126,7 +126,7 @@ class BMA180 : public device::SPI { public: BMA180(int bus, spi_dev_e device); - ~BMA180(); + virtual ~BMA180(); virtual int init(); diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 1350106539..e35bdb944a 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -86,7 +86,7 @@ class GPS : public device::CDev { public: GPS(const char* uart_path); - ~GPS(); + virtual ~GPS(); virtual int init(); diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index fe9b281f67..861ed79242 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -91,7 +91,7 @@ public: MODE_NONE }; HIL(); - ~HIL(); + virtual ~HIL(); virtual int ioctl(file *filp, int cmd, unsigned long arg); diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 4a201b98c6..8ab5682821 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -130,7 +130,7 @@ class HMC5883 : public device::I2C { public: HMC5883(int bus); - ~HMC5883(); + virtual ~HMC5883(); virtual int init(); diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index f2f585f42f..6227df72aa 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -152,7 +152,7 @@ class L3GD20 : public device::SPI { public: L3GD20(int bus, const char* path, spi_dev_e device); - ~L3GD20(); + virtual ~L3GD20(); virtual int init(); diff --git a/apps/drivers/led/led.cpp b/apps/drivers/led/led.cpp index 12d864be27..c7c45525e1 100644 --- a/apps/drivers/led/led.cpp +++ b/apps/drivers/led/led.cpp @@ -53,7 +53,7 @@ class LED : device::CDev { public: LED(); - ~LED(); + virtual ~LED(); virtual int init(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp index 406c726fb6..9d0f6bddcf 100644 --- a/apps/drivers/mb12xx/mb12xx.cpp +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -100,7 +100,7 @@ class MB12XX : public device::I2C { public: MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); - ~MB12XX(); + virtual ~MB12XX(); virtual int init(); diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 27e200e40b..ce7062046f 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -151,7 +151,7 @@ class MPU6000 : public device::SPI { public: MPU6000(int bus, spi_dev_e device); - ~MPU6000(); + virtual ~MPU6000(); virtual int init(); diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 44014d969b..08420822a5 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -104,7 +104,7 @@ class MS5611 : public device::I2C { public: MS5611(int bus); - ~MS5611(); + virtual ~MS5611(); virtual int init(); diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index c29fe0ba3d..8e13f7c62c 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -82,7 +82,7 @@ public: MODE_NONE }; PX4FMU(); - ~PX4FMU(); + virtual ~PX4FMU(); virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index f9ffa6bcdb..90320d3810 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -91,7 +91,7 @@ class PX4IO : public device::I2C { public: PX4IO(); - ~PX4IO(); + virtual ~PX4IO(); virtual int init(); diff --git a/apps/drivers/px4io/uploader.h b/apps/drivers/px4io/uploader.h index 915ee92597..f983d19811 100644 --- a/apps/drivers/px4io/uploader.h +++ b/apps/drivers/px4io/uploader.h @@ -46,7 +46,7 @@ class PX4IO_Uploader { public: PX4IO_Uploader(); - ~PX4IO_Uploader(); + virtual ~PX4IO_Uploader(); int upload(const char *filenames[]); diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 33ebe8600a..5a26855600 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -498,7 +498,7 @@ int mavlink_onboard_main(int argc, char *argv[]) mavlink_task = task_spawn("mavlink_onboard", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 6000 /* XXX probably excessive */, + 2048, mavlink_thread_main, (const char**)argv); exit(0); diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 00ddf15817..71386cba77 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -160,7 +160,7 @@ public: * @param control_cb Callback invoked when reading controls. */ Mixer(ControlCallback control_cb, uintptr_t cb_handle); - ~Mixer() {}; + virtual ~Mixer() {}; /** * Perform the mixing function. From a8a74fda96826f3fafa0a5ee4e3d8397ed4ca1e7 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 9 Mar 2013 21:58:30 +0100 Subject: [PATCH 62/65] Invert aileron actuator for correct aileron response in auto --- apps/controllib/fixedwing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index d9637b4a7a..b84a54fee5 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -322,7 +322,7 @@ void BlockMultiModeBacksideAutopilot::update() _att.roll, _att.pitch, _att.yaw, _att.rollspeed, _att.pitchspeed, _att.yawspeed ); - _actuators.control[CH_AIL] = - _backsideAutopilot.getAileron(); + _actuators.control[CH_AIL] = _backsideAutopilot.getAileron(); _actuators.control[CH_ELV] = - _backsideAutopilot.getElevator(); _actuators.control[CH_RDR] = _backsideAutopilot.getRudder(); _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); From 921ef9178d58ff83dcad44d0584b4f71fb2cae6f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Mar 2013 00:16:55 +0100 Subject: [PATCH 63/65] Hotfix: Correctly publish servo outputs --- apps/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 90320d3810..791964087c 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -1023,7 +1023,7 @@ PX4IO::io_publish_pwm_outputs() /* convert from register format to float */ for (unsigned i = 0; i < _max_actuators; i++) - outputs.output[i] = REG_TO_FLOAT(ctl[i]); + outputs.output[i] = ctl[i]; outputs.noutputs = _max_actuators; /* lazily advertise on first publication */ From c720a3238014e347fb84063a3fee2f9c812b3bf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Mar 2013 01:00:16 +0100 Subject: [PATCH 64/65] Hotfix: Correct channel order in HIL --- apps/mavlink/orb_listener.c | 20 ++------------------ 1 file changed, 2 insertions(+), 18 deletions(-) diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 9f85d5801f..58c709aecb 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -511,28 +511,12 @@ l_actuator_outputs(struct listener *l) 0); } else { - - /* - * Catch the case where no rudder is in use and throttle is not - * on output four - */ - float rudder, throttle; - - if (act_outputs.noutputs < 4) { - rudder = 0.0f; - throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; - - } else { - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; - } - mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 600.0f, (act_outputs.output[1] - 1500.0f) / 600.0f, - rudder, - throttle, + (act_outputs.output[2] - 1500.0f) / 600.0f, + (act_outputs.output[3] - 900.0f) / 1200.0f, (act_outputs.output[4] - 1500.0f) / 600.0f, (act_outputs.output[5] - 1500.0f) / 600.0f, (act_outputs.output[6] - 1500.0f) / 600.0f, From 1d444f80a3b9b575681e41b7a3a9b26a4b3d606d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 10 Mar 2013 22:01:13 +0100 Subject: [PATCH 65/65] Fixed comment --- apps/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 791964087c..27c885ed7a 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -122,7 +122,7 @@ private: uint16_t _alarms; /* subscribed topics */ - int _t_actuators; ///< actuator output topic + int _t_actuators; ///< actuator controls topic int _t_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic