From 90873474a9c52b66696f31c12b35b25ab0f6abd6 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 10 Sep 2013 22:58:44 +0200 Subject: multirotor_pos_control: setpint reset rewritten --- .../multirotor_pos_control.c | 117 ++++++++++++--------- src/modules/systemlib/pid/pid.c | 28 +++-- 2 files changed, 86 insertions(+), 59 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 336523072..1b3ddfdcd 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -212,17 +212,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - bool global_pos_sp_reproject = false; + bool reset_mission_sp = false; bool global_pos_sp_valid = false; - bool local_pos_sp_valid = false; - bool reset_sp_z = true; - bool reset_sp_xy = true; + bool reset_man_sp_z = true; + bool reset_man_sp_xy = true; bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; bool was_armed = false; - bool reset_integral = true; - bool reset_auto_pos = true; + bool reset_auto_sp_xy = true; + bool reset_auto_sp_z = true; + bool reset_takeoff_sp = true; hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; @@ -270,11 +270,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* use integral_limit_out = tilt_max / 2 */ float i_limit; - if (params.xy_vel_i == 0.0f) { + if (params.xy_vel_i > 0.0f) { i_limit = params.tilt_max / params.xy_vel_i / 2.0f; } else { - i_limit = 1.0f; // not used really + i_limit = 0.0f; // not used } pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); @@ -297,7 +297,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); global_pos_sp_valid = true; - global_pos_sp_reproject = true; + reset_mission_sp = true; } hrt_abstime t = hrt_absolute_time(); @@ -312,8 +312,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_armed && !was_armed) { /* reset setpoints and integrals on arming */ - reset_sp_z = true; - reset_sp_xy = true; + reset_man_sp_z = true; + reset_man_sp_xy = true; + reset_auto_sp_z = true; + reset_auto_sp_xy = true; + reset_takeoff_sp = true; reset_int_z = true; reset_int_xy = true; } @@ -348,8 +351,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { - if (reset_sp_z) { - reset_sp_z = false; + if (reset_man_sp_z) { + reset_man_sp_z = false; local_pos_sp.z = local_pos.z; mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } @@ -371,8 +374,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } if (control_mode.flag_control_position_enabled) { - if (reset_sp_xy) { - reset_sp_xy = false; + if (reset_man_sp_xy) { + reset_man_sp_xy = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; pid_reset_integral(&xy_vel_pids[0]); @@ -407,39 +410,43 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.yaw = att_sp.yaw_body; - /* local position setpoint is valid and can be used for loiter after position controlled mode */ - local_pos_sp_valid = control_mode.flag_control_position_enabled; - - reset_auto_pos = true; + /* local position setpoint is valid and can be used for auto loiter after position controlled mode */ + reset_auto_sp_xy = !control_mode.flag_control_position_enabled; + reset_auto_sp_z = !control_mode.flag_control_altitude_enabled; + reset_takeoff_sp = true; /* force reprojection of global setpoint after manual mode */ - global_pos_sp_reproject = true; + reset_mission_sp = true; } else if (control_mode.flag_control_auto_enabled) { /* AUTO mode, use global setpoint */ if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { - reset_auto_pos = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_auto_pos) { + if (reset_takeoff_sp) { + reset_takeoff_sp = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; - reset_auto_pos = false; mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); } + reset_auto_sp_xy = false; + reset_auto_sp_z = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { // TODO - reset_auto_pos = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { /* init local projection using local position ref */ if (local_pos.ref_timestamp != local_ref_timestamp) { - global_pos_sp_reproject = true; + reset_mission_sp = true; local_ref_timestamp = local_pos.ref_timestamp; double lat_home = local_pos.ref_lat * 1e-7; double lon_home = local_pos.ref_lon * 1e-7; @@ -447,9 +454,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home); } - if (global_pos_sp_reproject) { + if (reset_mission_sp) { + reset_mission_sp = false; /* update global setpoint projection */ - global_pos_sp_reproject = false; if (global_pos_sp_valid) { /* global position setpoint valid, use it */ @@ -471,33 +478,43 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y); } else { - if (!local_pos_sp_valid) { + if (reset_auto_sp_xy) { + reset_auto_sp_xy = false; /* local position setpoint is invalid, * use current position as setpoint for loiter */ local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; + att_sp.yaw_body = global_pos_sp.yaw; + } + + if (reset_auto_sp_z) { + reset_auto_sp_z = false; + local_pos_sp.z = local_pos.z; } mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } } - reset_auto_pos = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; + } + + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + reset_takeoff_sp = true; } if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - global_pos_sp_reproject = true; + reset_mission_sp = true; } /* reset setpoints after AUTO mode */ - reset_sp_xy = true; - reset_sp_z = true; + reset_man_sp_xy = true; + reset_man_sp_z = true; } else { - /* no control, loiter or stay on ground */ + /* no control (failsafe), loiter or stay on ground */ if (local_pos.landed) { /* landed: move setpoint down */ /* in air: hold altitude */ @@ -508,27 +525,30 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z); } - reset_sp_z = true; + reset_man_sp_z = true; } else { /* in air: hold altitude */ - if (reset_sp_z) { - reset_sp_z = false; + if (reset_man_sp_z) { + reset_man_sp_z = false; local_pos_sp.z = local_pos.z; mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); } + + reset_auto_sp_z = false; } if (control_mode.flag_control_position_enabled) { - if (reset_sp_xy) { - reset_sp_xy = false; + if (reset_man_sp_xy) { + reset_man_sp_xy = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); } + + reset_auto_sp_xy = false; } } @@ -540,7 +560,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; } else { - reset_sp_z = true; + reset_man_sp_z = true; global_vel_sp.vz = 0.0f; } @@ -558,7 +578,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } else { - reset_sp_xy = true; + reset_man_sp_xy = true; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } @@ -640,12 +660,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* position controller disabled, reset setpoints */ - reset_sp_z = true; - reset_sp_xy = true; + reset_man_sp_z = true; + reset_man_sp_xy = true; reset_int_z = true; reset_int_xy = true; - global_pos_sp_reproject = true; - reset_auto_pos = true; + reset_mission_sp = true; + reset_auto_sp_xy = true; + reset_auto_sp_z = true; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 739503ed4..77c952f52 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -167,20 +167,26 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - // Calculate the error integral and check for saturation - i = pid->integral + (error * dt); - - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; + if (pid->ki > 0.0f) { + // Calculate the error integral and check for saturation + i = pid->integral + (error * dt); + + if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { + i = pid->integral; // If saturated then do not update integral value + pid->saturated = 1; + + } else { + if (!isfinite(i)) { + i = 0.0f; + } - } else { - if (!isfinite(i)) { - i = 0.0f; + pid->integral = i; + pid->saturated = 0; } - pid->integral = i; + } else { + i = 0.0f; pid->saturated = 0; } -- cgit v1.2.3 From bbac1445b0ab8efc914399ba0c41116e0854c729 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sun, 1 Sep 2013 13:25:57 -0700 Subject: Add DMA buffer allocation pool. --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 61 ++++++++++++++++++++++++++++- 1 file changed, 60 insertions(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index 135767b26..c5d0377bc 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include "board_config.h" @@ -69,6 +70,7 @@ #include #include +#include /**************************************************************************** * Pre-Processor Definitions @@ -76,6 +78,10 @@ /* Configuration ************************************************************/ +#if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN and CONFIG_FAT_DMAMEMORY +#endif + /* Debug ********************************************************************/ #ifdef CONFIG_CPP_HAVE_VARARGS @@ -96,10 +102,59 @@ * Protected Functions ****************************************************************************/ +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + /**************************************************************************** * Public Functions ****************************************************************************/ +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + /************************************************************************************ * Name: stm32_boardinitialize * @@ -110,7 +165,8 @@ * ************************************************************************************/ -__EXPORT void stm32_boardinitialize(void) +__EXPORT void +stm32_boardinitialize(void) { /* configure SPI interfaces */ stm32_spiinitialize(); @@ -170,6 +226,9 @@ __EXPORT int nsh_archinitialize(void) /* configure the high-resolution time/callout interface */ hrt_init(); + /* configure the DMA allocator */ + dma_alloc_init(); + /* configure CPU load estimation */ #ifdef CONFIG_SCHED_INSTRUMENTATION cpuload_initialize_once(); -- cgit v1.2.3 From f49e444ce3b074919ebd187983be5ff0913c5111 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 3 Sep 2013 21:21:07 -0700 Subject: Defconfig hacks to get me a console. --- nuttx-configs/px4fmu-v2/nsh/defconfig | 125 +++++++++++++++++----------------- 1 file changed, 61 insertions(+), 64 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 0615950a2..70adf6d9d 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -38,7 +38,31 @@ CONFIG_ARCH_MATH_H=y # # Debug Options # -# CONFIG_DEBUG is not set +CONFIG_DEBUG=y +CONFIG_DEBUG_VERBOSE=y + +# +# Subsystem Debug Options +# +# CONFIG_DEBUG_MM is not set +# CONFIG_DEBUG_SCHED is not set +# CONFIG_DEBUG_USB is not set +CONFIG_DEBUG_FS=y +# CONFIG_DEBUG_LIB is not set +# CONFIG_DEBUG_BINFMT is not set +# CONFIG_DEBUG_GRAPHICS is not set + +# +# Driver Debug Options +# +# CONFIG_DEBUG_ANALOG is not set +# CONFIG_DEBUG_I2C is not set +# CONFIG_DEBUG_SPI is not set +# CONFIG_DEBUG_SDIO is not set +# CONFIG_DEBUG_GPIO is not set +CONFIG_DEBUG_DMA=y +# CONFIG_DEBUG_WATCHDOG is not set +# CONFIG_DEBUG_AUDIO is not set CONFIG_DEBUG_SYMBOLS=y # @@ -86,6 +110,7 @@ CONFIG_ARCH_HAVE_FPU=y CONFIG_ARCH_FPU=y CONFIG_ARCH_HAVE_MPU=y # CONFIG_ARMV7M_MPU is not set +# CONFIG_DEBUG_HARDFAULT is not set # # ARMV7M Configuration Options @@ -94,7 +119,9 @@ CONFIG_ARCH_HAVE_MPU=y CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y CONFIG_ARMV7M_STACKCHECK=y CONFIG_SERIAL_TERMIOS=y -CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_SDIO_DMA=y +CONFIG_SDIO_DMAPRIO=0x00010000 +# CONFIG_SDIO_WIDTH_D1_ONLY is not set # # STM32 Configuration Options @@ -240,9 +267,6 @@ CONFIG_STM32_ADC=y CONFIG_STM32_SPI=y CONFIG_STM32_I2C=y -CONFIG_ARCH_HAVE_LEDS=y -# CONFIG_ARCH_LEDS is not set - # # Alternate Pin Mapping # @@ -254,7 +278,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=y CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y # CONFIG_STM32_FORCEPOWER is not set # CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set -CONFIG_STM32_CCMEXCLUDE=y +# CONFIG_STM32_CCMEXCLUDE is not set CONFIG_STM32_DMACAPABLE=y # CONFIG_STM32_TIM1_PWM is not set # CONFIG_STM32_TIM3_PWM is not set @@ -271,40 +295,22 @@ CONFIG_STM32_USART=y # U[S]ART Configuration # # CONFIG_USART1_RS485 is not set -CONFIG_USART1_RXDMA=y +# CONFIG_USART1_RXDMA is not set # CONFIG_USART2_RS485 is not set CONFIG_USART2_RXDMA=y # CONFIG_USART3_RS485 is not set # CONFIG_USART3_RXDMA is not set # CONFIG_UART4_RS485 is not set # CONFIG_UART4_RXDMA is not set -# CONFIG_UART5_RXDMA is not set # CONFIG_USART6_RS485 is not set # CONFIG_USART6_RXDMA is not set -# CONFIG_USART7_RXDMA is not set -CONFIG_USART8_RXDMA=y +# CONFIG_UART7_RS485 is not set +# CONFIG_UART7_RXDMA is not set +# CONFIG_UART8_RS485 is not set +# CONFIG_UART8_RXDMA is not set +CONFIG_SERIAL_DISABLE_REORDERING=y CONFIG_STM32_USART_SINGLEWIRE=y - -# Previous: -## CONFIG_USART1_RS485 is not set -#CONFIG_USART1_RXDMA=y -## CONFIG_USART2_RS485 is not set -#CONFIG_USART2_RXDMA=y -## CONFIG_USART3_RS485 is not set -#CONFIG_USART3_RXDMA=y -## CONFIG_UART4_RS485 is not set -#CONFIG_UART4_RXDMA=y -## CONFIG_UART5_RXDMA is not set -## CONFIG_USART6_RS485 is not set -## CONFIG_USART6_RXDMA is not set -## CONFIG_USART7_RXDMA is not set -#CONFIG_USART8_RXDMA=y -#CONFIG_STM32_USART_SINGLEWIRE=y - - - - # # SPI Configuration # @@ -323,25 +329,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # # SDIO Configuration # - -# -# Maintain legacy build behavior (revisit) -# - -CONFIG_MMCSD=y -#CONFIG_MMCSD_SPI=y -CONFIG_MMCSD_SDIO=y -CONFIG_MTD=y - -# -# STM32 SDIO-based MMC/SD driver -# -CONFIG_SDIO_DMA=y -# CONFIG_SDIO_DMAPRIO is not set -# CONFIG_SDIO_WIDTH_D1_ONLY is not set -CONFIG_MMCSD_MULTIBLOCK_DISABLE=y -CONFIG_MMCSD_MMCSUPPORT=n -CONFIG_MMCSD_HAVECARDDETECT=n +CONFIG_SDIO_PRI=128 # # USB Host Configuration @@ -393,15 +381,14 @@ CONFIG_BOOT_RUNFROMFLASH=y # # Board Selection # -CONFIG_ARCH_BOARD_PX4FMU_V2=y -# CONFIG_ARCH_BOARD_CUSTOM is not set -CONFIG_ARCH_BOARD="px4fmu-v2" +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" # # Common Board Options # -CONFIG_NSH_MMCSDSLOTNO=0 CONFIG_NSH_MMCSDMINOR=0 +CONFIG_NSH_MMCSDSLOTNO=0 # # Board-Specific Options @@ -497,7 +484,16 @@ CONFIG_WATCHDOG=y # CONFIG_BCH is not set # CONFIG_INPUT is not set # CONFIG_LCD is not set -# CONFIG_MMCSD is not set +CONFIG_MMCSD=y +CONFIG_MMCSD_NSLOTS=1 +# CONFIG_MMCSD_READONLY is not set +CONFIG_MMCSD_MULTIBLOCK_DISABLE=y +# CONFIG_MMCSD_MMCSUPPORT is not set +# CONFIG_MMCSD_HAVECARDDETECT is not set +# CONFIG_MMCSD_SPI is not set +CONFIG_MMCSD_SDIO=y +# CONFIG_SDIO_MUXBUS is not set +# CONFIG_SDIO_BLOCKSETUP is not set CONFIG_MTD=y # @@ -523,7 +519,7 @@ CONFIG_PIPES=y # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -CONFIG_DEV_LOWCONSOLE=y +# CONFIG_DEV_LOWCONSOLE is not set CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_UART4=y @@ -536,6 +532,7 @@ CONFIG_ARCH_HAVE_USART6=y CONFIG_MCU_SERIAL=y CONFIG_STANDARD_SERIAL=y CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_SERIAL_TIOCSERGSTRUCT is not set # CONFIG_USART1_SERIAL_CONSOLE is not set # CONFIG_USART2_SERIAL_CONSOLE is not set # CONFIG_USART3_SERIAL_CONSOLE is not set @@ -640,7 +637,6 @@ CONFIG_USBDEV=y # CONFIG_USBDEV_SELFPOWERED is not set CONFIG_USBDEV_BUSPOWERED=y CONFIG_USBDEV_MAXPOWER=500 -# CONFIG_USBDEV_REMOTEWAKEUP is not set # CONFIG_USBDEV_DMA is not set # CONFIG_USBDEV_TRACE is not set @@ -650,7 +646,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=n +# CONFIG_CDCACM_CONSOLE is not set CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 @@ -702,7 +698,7 @@ CONFIG_FAT_LCNAMES=y CONFIG_FAT_LFN=y CONFIG_FAT_MAXFNAME=32 CONFIG_FS_FATTIME=y -# CONFIG_FAT_DMAMEMORY is not set +CONFIG_FAT_DMAMEMORY=y CONFIG_FS_NXFFS=y CONFIG_NXFFS_PREALLOCATED=y CONFIG_NXFFS_ERASEDSTATE=0xff @@ -716,10 +712,8 @@ CONFIG_FS_BINFS=y # # System Logging # -CONFIG_SYSLOG_ENABLE=y -CONFIG_SYSLOG=y -CONFIG_SYSLOG_CHAR=y -CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set # # Graphics Support @@ -733,8 +727,9 @@ CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" # CONFIG_MM_SMALL is not set CONFIG_MM_REGIONS=2 CONFIG_GRAN=y -CONFIG_GRAN_SINGLE=y -CONFIG_GRAN_INTR=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set +# CONFIG_DEBUG_GRAN is not set # # Audio Support @@ -1002,6 +997,7 @@ CONFIG_NSH_CONSOLE=y # # USB Trace Support # +# CONFIG_NSH_USBDEV_TRACE is not set # CONFIG_NSH_CONDEV is not set CONFIG_NSH_ARCHINIT=y @@ -1031,6 +1027,7 @@ CONFIG_NSH_ARCHINIT=y # # FLASH Erase-all Command # +# CONFIG_SYSTEM_FLASH_ERASEALL is not set # # readline() -- cgit v1.2.3 From ed4b34547c1bddeb696b9e3c46bdb15407a845c9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 3 Sep 2013 21:21:41 -0700 Subject: Make the init code compile if we don't have the granule allocator / dma allocator required --- src/drivers/boards/px4fmu-v2/px4fmu2_init.c | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c index c5d0377bc..ae2a645f7 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_init.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_init.c @@ -78,10 +78,6 @@ /* Configuration ************************************************************/ -#if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) -# error microSD DMA support requires CONFIG_GRAN and CONFIG_FAT_DMAMEMORY -#endif - /* Debug ********************************************************************/ #ifdef CONFIG_CPP_HAVE_VARARGS @@ -102,6 +98,11 @@ * Protected Functions ****************************************************************************/ +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + static GRAN_HANDLE dma_allocator; /* @@ -155,6 +156,12 @@ fat_dma_free(FAR void *memory, size_t size) gran_free(dma_allocator, memory, size); } +#else + +# define dma_alloc_init() + +#endif + /************************************************************************************ * Name: stm32_boardinitialize * -- cgit v1.2.3 From 514d32e961e37f68443871dd93f4ce4c89c4aad9 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 3 Sep 2013 21:22:03 -0700 Subject: Cut down 'tests file' for debugging --- src/systemcmds/tests/tests_file.c | 43 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 2 deletions(-) diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index 47f480758..588d648bd 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -52,6 +52,44 @@ int test_file(int argc, char *argv[]) { + const iterations = 10; + + /* check if microSD card is mounted */ + struct stat buffer; + if (stat("/fs/microsd/", &buffer)) { + warnx("no microSD card mounted, aborting file test"); + return 1; + } + + uint8_t buf[512]; + hrt_abstime start, end; + perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes"); + + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + memset(buf, 0, sizeof(buf)); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + perf_begin(wperf); + write(fd, buf, sizeof(buf)); + perf_end(wperf); + } + end = hrt_absolute_time(); + + close(fd); + + warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + perf_print_counter(wperf); + perf_free(wperf); + + return 0; +} +#if 0 +int +test_file(int argc, char *argv[]) +{ + const iterations = 1024; + /* check if microSD card is mounted */ struct stat buffer; if (stat("/fs/microsd/", &buffer)) { @@ -67,7 +105,7 @@ test_file(int argc, char *argv[]) memset(buf, 0, sizeof(buf)); start = hrt_absolute_time(); - for (unsigned i = 0; i < 1024; i++) { + for (unsigned i = 0; i < iterations; i++) { perf_begin(wperf); write(fd, buf, sizeof(buf)); perf_end(wperf); @@ -78,7 +116,7 @@ test_file(int argc, char *argv[]) unlink("/fs/microsd/testfile"); - warnx("512KiB in %llu microseconds", end - start); + warnx("%dKiB in %llu microseconds", iterations / 2, end - start); perf_print_counter(wperf); perf_free(wperf); @@ -112,3 +150,4 @@ test_file(int argc, char *argv[]) return 0; } +#endif -- cgit v1.2.3 From 3b8039e4e009868ec7722ad559cd5b62c5f7a325 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 11 Sep 2013 22:11:37 -0400 Subject: Implement message based receiver pairing --- src/drivers/px4io/px4io.cpp | 32 ++++++++++++++++++++++++++++--- src/modules/uORB/topics/vehicle_command.h | 3 ++- 2 files changed, 31 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 78d1d3e63..c93904a3f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -254,6 +254,7 @@ private: int _t_actuator_armed; ///< system armed control topic int _t_vehicle_control_mode;///< vehicle control mode topic int _t_param; ///< parameter update topic + int _t_vehicle_command; ///< vehicle command topic /* advertised topics */ orb_advert_t _to_input_rc; ///< rc inputs from io @@ -440,6 +441,7 @@ PX4IO::PX4IO(device::Device *interface) : _t_actuator_armed(-1), _t_vehicle_control_mode(-1), _t_param(-1), + _t_vehicle_command(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), @@ -732,16 +734,20 @@ PX4IO::task_main() _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ + _t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command)); + orb_set_interval(_t_param, 1000); /* 1Hz update rate max. */ + if ((_t_actuators < 0) || (_t_actuator_armed < 0) || (_t_vehicle_control_mode < 0) || - (_t_param < 0)) { + (_t_param < 0) || + (_t_vehicle_command < 0)) { log("subscription(s) failed"); goto out; } /* poll descriptor */ - pollfd fds[4]; + pollfd fds[5]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; fds[1].fd = _t_actuator_armed; @@ -750,8 +756,10 @@ PX4IO::task_main() fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; + fds[4].fd = _t_vehicle_command; + fds[4].events = POLLIN; - debug("ready"); + log("ready"); /* lock against the ioctl handler */ lock(); @@ -791,6 +799,24 @@ PX4IO::task_main() if ((fds[1].revents & POLLIN) || (fds[2].revents & POLLIN)) io_set_arming_state(); + /* if we have a vehicle command, handle it */ + if (fds[4].revents & POLLIN) { + struct vehicle_command_s cmd; + orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); + if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { + if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if ((cmd.param2 == 0.0f) || (cmd.param2 == 1.0f)) { + mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", (cmd.param2 == 0.0f) == 1 ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, (cmd.param2 == 0.0f) ? 3 : 7); + } else { + mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); + } + } else { + mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); + } + } + } + /* * If it's time for another tick of the polling status machine, * try it now. diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 31ff014de..a62e38db2 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -86,7 +86,8 @@ enum VEHICLE_CMD VEHICLE_CMD_OVERRIDE_GOTO=252, /* Hold / continue the current action |MAV_GOTO_DO_HOLD: hold MAV_GOTO_DO_CONTINUE: continue with next item in mission plan| MAV_GOTO_HOLD_AT_CURRENT_POSITION: Hold at current position MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position| MAV_FRAME coordinate frame of hold point| Desired yaw angle in degrees| Latitude / X position| Longitude / Y position| Altitude / Z position| */ VEHICLE_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ VEHICLE_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ - VEHICLE_CMD_ENUM_END=401, /* | */ + VEHICLE_CMD_START_RX_PAIR=500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ + VEHICLE_CMD_ENUM_END=501, /* | */ }; /** -- cgit v1.2.3 From 41982579b3825bf93dad46ec6eed383ce47f04ff Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 11 Sep 2013 22:54:23 -0400 Subject: Refactor dsm binding code in px4io.cpp - Move repeated code into member function --- src/drivers/px4io/px4io.cpp | 54 +++++++++++++++++++++++++++------------------ 1 file changed, 32 insertions(+), 22 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index c93904a3f..56a294098 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -241,7 +241,8 @@ private: volatile int _task; /// Date: Thu, 12 Sep 2013 10:03:54 +0200 Subject: Disabling debug output for further testing --- nuttx-configs/px4fmu-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 70adf6d9d..8e2ae2d00 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -38,8 +38,8 @@ CONFIG_ARCH_MATH_H=y # # Debug Options # -CONFIG_DEBUG=y -CONFIG_DEBUG_VERBOSE=y +CONFIG_DEBUG=n +CONFIG_DEBUG_VERBOSE=n # # Subsystem Debug Options -- cgit v1.2.3 From d84fe2913e40e91ce6d7530b438dbe18db49ec01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Sep 2013 01:34:49 +0200 Subject: Move IRQ restore to right position --- src/drivers/lsm303d/lsm303d.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a90cd0a3d..0b0906d9e 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -532,6 +532,7 @@ LSM303D::reset() /* enable mag */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + irqrestore(flags); accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); @@ -540,7 +541,6 @@ LSM303D::reset() mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); - irqrestore(flags); _accel_read = 0; _mag_read = 0; @@ -1013,6 +1013,7 @@ LSM303D::accel_set_range(unsigned max_g) _accel_range_scale = new_scale_g_digit * LSM303D_ONE_G; + modify_reg(ADDR_CTRL_REG2, clearbits, setbits); return OK; -- cgit v1.2.3 From 41610ff7dd6233853270e921828c815797fd6aeb Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 12 Sep 2013 21:36:20 -0400 Subject: DSM pairing cleanup in px4io.cpp - Simplify parameter range checking in dsm_bind_ioctl - Replace DSM magic numbers with symbolic constants --- src/drivers/drv_pwm_output.h | 3 +++ src/drivers/px4io/px4io.cpp | 26 +++++++++++++------------- src/modules/sensors/sensor_params.c | 2 +- 3 files changed, 17 insertions(+), 14 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index ec9d4ca09..94e923d71 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -118,6 +118,9 @@ ORB_DECLARE(output_pwm); /** start DSM bind */ #define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) +#define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ +#define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ + /** Power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 56a294098..6e3a39aae 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -415,10 +415,9 @@ private: /** * Handle issuing dsm bind ioctl to px4io. * - * @param valid true: the dsm mode is in range - * @param isDsm2 true: dsm2m false: dsmx + * @param dsmMode 0:dsm2, 1:dsmx */ - void dsm_bind_ioctl(bool valid, bool isDsm2); + void dsm_bind_ioctl(int dsmMode); }; @@ -816,7 +815,7 @@ PX4IO::task_main() orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd); // Check for a DSM pairing command if ((cmd.command == VEHICLE_CMD_START_RX_PAIR) && (cmd.param1== 0.0f)) { - dsm_bind_ioctl((cmd.param2 == 0.0f) || (cmd.param2 == 1.0f), cmd.param2 == 0.0f); + dsm_bind_ioctl((int)cmd.param2); } } @@ -857,8 +856,8 @@ PX4IO::task_main() // See if bind parameter has been set, and reset it to 0 param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); if (dsm_bind_val) { - dsm_bind_ioctl((dsm_bind_val == 1) || (dsm_bind_val == 2), dsm_bind_val == 1); - dsm_bind_val = 0; + dsm_bind_ioctl(dsm_bind_val); + dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); } @@ -1166,14 +1165,15 @@ PX4IO::io_handle_status(uint16_t status) } void -PX4IO::dsm_bind_ioctl(bool valid, bool isDsm2) +PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { - if (valid) { - mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", isDsm2 ? '2' : 'x'); - ioctl(nullptr, DSM_BIND_START, isDsm2 ? 3 : 7); + /* 0: dsm2, 1:dsmx */ + if ((dsmMode >= 0) && (dsmMode <= 1)) { + mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] invalid bind type, bind request rejected"); + mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); } } else { mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); @@ -2035,9 +2035,9 @@ bind(int argc, char *argv[]) errx(0, "needs argument, use dsm2 or dsmx"); if (!strcmp(argv[2], "dsm2")) - pulses = 3; + pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) - pulses = 7; + pulses = DSMX_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 992abf2cc..950af2eba 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -166,7 +166,7 @@ PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ -PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ +PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); -- cgit v1.2.3 From 19fdaf2009d41885923b586432cb2506a24ca5b3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 12 Sep 2013 23:56:53 -0700 Subject: Use the generic device::SPI locking strategy. --- src/drivers/device/spi.cpp | 34 +++++++++++++++++++++++++++++----- src/drivers/device/spi.h | 11 +++++++++++ src/drivers/l3gd20/l3gd20.cpp | 3 --- src/drivers/lsm303d/lsm303d.cpp | 5 ----- src/drivers/ms5611/ms5611_spi.cpp | 16 +--------------- 5 files changed, 41 insertions(+), 28 deletions(-) diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index 8fffd60cb..fa6b78d64 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -67,6 +67,7 @@ SPI::SPI(const char *name, CDev(name, devname, irq), // public // protected + locking_mode(LOCK_PREEMPTION), // private _bus(bus), _device(device), @@ -132,13 +133,25 @@ SPI::probe() int SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { + irqstate_t state; if ((send == nullptr) && (recv == nullptr)) return -EINVAL; - /* do common setup */ - if (!up_interrupt_context()) - SPI_LOCK(_dev, true); + /* lock the bus as required */ + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + state = irqsave(); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, true); + break; + case LOCK_NONE: + break; + } + } SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); @@ -151,8 +164,19 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); - if (!up_interrupt_context()) - SPI_LOCK(_dev, false); + if (!up_interrupt_context()) { + switch (locking_mode) { + default: + case LOCK_PREEMPTION: + irqrestore(state); + break; + case LOCK_THREADS: + SPI_LOCK(_dev, false); + break; + case LOCK_NONE: + break; + } + } return OK; } diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index e0122372a..9103dca2e 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -101,6 +101,17 @@ protected: */ int transfer(uint8_t *send, uint8_t *recv, unsigned len); + /** + * Locking modes supported by the driver. + */ + enum LockMode { + LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ + LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ + LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ + }; + + LockMode locking_mode; /**< selected locking mode */ + private: int _bus; enum spi_dev_e _device; diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4c3b0ce51..748809d3f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -367,7 +367,6 @@ out: int L3GD20::probe() { - irqstate_t flags = irqsave(); /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); @@ -393,8 +392,6 @@ L3GD20::probe() success = true; } - irqrestore(flags); - if (success) return OK; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a90cd0a3d..fcdf2768b 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -525,7 +525,6 @@ out: void LSM303D::reset() { - irqstate_t flags = irqsave(); /* enable accel*/ write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE); @@ -540,7 +539,6 @@ LSM303D::reset() mag_set_range(LSM303D_MAG_DEFAULT_RANGE_GA); mag_set_samplerate(LSM303D_MAG_DEFAULT_RATE); - irqrestore(flags); _accel_read = 0; _mag_read = 0; @@ -549,15 +547,12 @@ LSM303D::reset() int LSM303D::probe() { - irqstate_t flags = irqsave(); /* read dummy value to void to clear SPI statemachine on sensor */ (void)read_reg(ADDR_WHO_AM_I); /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - irqrestore(flags); - if (success) return OK; diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 21caed2ff..e547c913b 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -142,23 +142,15 @@ MS5611_SPI::init() goto out; } - /* disable interrupts, make this section atomic */ - flags = irqsave(); /* send reset command */ ret = _reset(); - /* re-enable interrupts */ - irqrestore(flags); if (ret != OK) { debug("reset failed"); goto out; } - /* disable interrupts, make this section atomic */ - flags = irqsave(); /* read PROM */ ret = _read_prom(); - /* re-enable interrupts */ - irqrestore(flags); if (ret != OK) { debug("prom readout failed"); goto out; @@ -270,13 +262,7 @@ MS5611_SPI::_reg16(unsigned reg) int MS5611_SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) { - irqstate_t flags = irqsave(); - - int ret = transfer(send, recv, len); - - irqrestore(flags); - - return ret; + return transfer(send, recv, len); } #endif /* PX4_SPIDEV_BARO */ -- cgit v1.2.3 From e3864e7dbbe03a4ae7bdc9eb5ea12123477dd07f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Sep 2013 09:55:06 +0200 Subject: Trust on the microSD for params for now --- ROMFS/px4fmu_common/init.d/rcS | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4c4b0129e..cd4d4487d 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -73,20 +73,20 @@ then # # Load microSD params # - if ramtron start - then - param select /ramtron/params - if [ -f /ramtron/params ] - then - param load /ramtron/params - fi - else + #if ramtron start + #then + # param select /ramtron/params + # if [ -f /ramtron/params ] + # then + # param load /ramtron/params + # fi + #else param select /fs/microsd/params if [ -f /fs/microsd/params ] then param load /fs/microsd/params fi - fi + #fi # # Start system state indicator -- cgit v1.2.3 From 24648b529402abb665c1b7c0064a9ee3150999fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Sep 2013 10:16:32 +0200 Subject: Minor cleanups in the drivers --- src/drivers/hmc5883/hmc5883.cpp | 4 ---- src/drivers/lsm303d/lsm303d.cpp | 1 - 2 files changed, 5 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 58a1593ed..5e891d7bb 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1292,10 +1292,6 @@ test() if (fd < 0) err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH); - /* set the queue depth to 10 */ - if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) - errx(1, "failed to set queue depth"); - /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 83919d263..2c56b6035 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -531,7 +531,6 @@ LSM303D::reset() /* enable mag */ write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - irqrestore(flags); accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); -- cgit v1.2.3 From ecbb319153955dac3287548703d40ba866ddf070 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 13 Sep 2013 15:06:59 +0200 Subject: Disable low console for v2, should not be enabled in parallel with normal console --- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 0615950a2..2867f9113 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -523,7 +523,7 @@ CONFIG_PIPES=y # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -CONFIG_DEV_LOWCONSOLE=y +# CONFIG_DEV_LOWCONSOLE is not set CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_UART4=y -- cgit v1.2.3 From 8a70efdf43706b42e552a131e332789c114c7d4b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Sep 2013 08:55:45 +0200 Subject: Beep and print message when arming is not allowed --- src/modules/commander/commander.cpp | 104 ++++++++++++++++++++++-------------- 1 file changed, 65 insertions(+), 39 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5eeb018fd..a2443b0f6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,7 +369,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + } if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); @@ -561,7 +567,7 @@ int commander_thread_main(int argc, char *argv[]) /* flags for control apps */ struct vehicle_control_mode_s control_mode; - + /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -1083,10 +1089,18 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - status.main_state == MAIN_STATE_MANUAL && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + + } else { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + } + stick_on_counter = 0; } else { @@ -1106,15 +1120,8 @@ int commander_thread_main(int argc, char *argv[]) } } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates safety switch not pressed */ - - if (!(!safety.safety_switch_available || safety.safety_off)) { - print_reject_arm("NOT ARMING: Press safety switch first."); - - } else { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - } + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } /* fill current_status according to mode switches */ @@ -1228,6 +1235,7 @@ int commander_thread_main(int argc, char *argv[]) /* wait for threads to complete */ ret = pthread_join(commander_low_prio_thread, NULL); + if (ret) { warn("join failed", ret); } @@ -1295,8 +1303,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (armed->armed) { rgbled_set_mode(RGBLED_MODE_ON); + } else if (armed->ready_to_arm) { rgbled_set_mode(RGBLED_MODE_BREATHE); + } else { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } @@ -1304,29 +1314,35 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { switch (status->battery_warning) { - case VEHICLE_BATTERY_WARNING_LOW: - rgbled_set_color(RGBLED_COLOR_YELLOW); - break; - case VEHICLE_BATTERY_WARNING_CRITICAL: - rgbled_set_color(RGBLED_COLOR_AMBER); - break; - default: - break; + case VEHICLE_BATTERY_WARNING_LOW: + rgbled_set_color(RGBLED_COLOR_YELLOW); + break; + + case VEHICLE_BATTERY_WARNING_CRITICAL: + rgbled_set_color(RGBLED_COLOR_AMBER); + break; + + default: + break; } + } else { switch (status->main_state) { - case MAIN_STATE_MANUAL: - rgbled_set_color(RGBLED_COLOR_WHITE); - break; - case MAIN_STATE_SEATBELT: - case MAIN_STATE_EASY: - rgbled_set_color(RGBLED_COLOR_GREEN); - break; - case MAIN_STATE_AUTO: - rgbled_set_color(RGBLED_COLOR_BLUE); - break; - default: - break; + case MAIN_STATE_MANUAL: + rgbled_set_color(RGBLED_COLOR_WHITE); + break; + + case MAIN_STATE_SEATBELT: + case MAIN_STATE_EASY: + rgbled_set_color(RGBLED_COLOR_GREEN); + break; + + case MAIN_STATE_AUTO: + rgbled_set_color(RGBLED_COLOR_BLUE); + break; + + default: + break; } } @@ -1497,16 +1513,18 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c return TRANSITION_NOT_CHANGED; } } + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && - status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && - status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && - status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { /* possibly on ground, switch to TAKEOFF if needed */ if (local_pos->z > -takeoff_alt || status->condition_landed) { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); return res; } } + /* switch to AUTO mode */ if (status->rc_signal_found_once && !status->rc_signal_lost) { /* act depending on switches when manual control enabled */ @@ -1524,18 +1542,20 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } + } else { /* switch to MISSION when no RC control and first time in some AUTO mode */ if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { res = TRANSITION_NOT_CHANGED; } else { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } + } else { /* disarmed, always switch to AUTO_READY */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1762,35 +1782,41 @@ void *commander_low_prio_loop(void *arg) if (((int)(cmd.param1)) == 0) { int ret = param_load_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } else if (((int)(cmd.param1)) == 1) { int ret = param_save_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } -- cgit v1.2.3 From a0d26cb282145f553aaf2f65d5feadd5c89941e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 14 Sep 2013 16:25:38 +0200 Subject: Make mixer upload not depending on single serial transfer error --- src/drivers/px4io/px4io.cpp | 85 +++++++++++++++++++++++++-------------------- 1 file changed, 47 insertions(+), 38 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6e3a39aae..3e83fcad8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -392,7 +392,7 @@ private: /** * Send mixer definition text to IO */ - int mixer_send(const char *buf, unsigned buflen); + int mixer_send(const char *buf, unsigned buflen, unsigned retries=3); /** * Handle a status update from IO. @@ -1468,61 +1468,70 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t } int -PX4IO::mixer_send(const char *buf, unsigned buflen) +PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) { - uint8_t frame[_max_transfer]; - px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; - unsigned max_len = _max_transfer - sizeof(px4io_mixdata); - msg->f2i_mixer_magic = F2I_MIXER_MAGIC; - msg->action = F2I_MIXER_ACTION_RESET; + uint8_t frame[_max_transfer]; do { - unsigned count = buflen; - if (count > max_len) - count = max_len; + px4io_mixdata *msg = (px4io_mixdata *)&frame[0]; + unsigned max_len = _max_transfer - sizeof(px4io_mixdata); - if (count > 0) { - memcpy(&msg->text[0], buf, count); - buf += count; - buflen -= count; - } + msg->f2i_mixer_magic = F2I_MIXER_MAGIC; + msg->action = F2I_MIXER_ACTION_RESET; - /* - * We have to send an even number of bytes. This - * will only happen on the very last transfer of a - * mixer, and we are guaranteed that there will be - * space left to round up as _max_transfer will be - * even. - */ - unsigned total_len = sizeof(px4io_mixdata) + count; - if (total_len % 1) { - msg->text[count] = '\0'; - total_len++; - } + do { + unsigned count = buflen; - int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + if (count > max_len) + count = max_len; - if (ret) { - log("mixer send error %d", ret); - return ret; - } + if (count > 0) { + memcpy(&msg->text[0], buf, count); + buf += count; + buflen -= count; + } - msg->action = F2I_MIXER_ACTION_APPEND; + /* + * We have to send an even number of bytes. This + * will only happen on the very last transfer of a + * mixer, and we are guaranteed that there will be + * space left to round up as _max_transfer will be + * even. + */ + unsigned total_len = sizeof(px4io_mixdata) + count; + if (total_len % 1) { + msg->text[count] = '\0'; + total_len++; + } + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + + if (ret) { + log("mixer send error %d", ret); + return ret; + } + + msg->action = F2I_MIXER_ACTION_APPEND; - } while (buflen > 0); + } while (buflen > 0); + + retries--; + + log("mixer sent"); + + } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); /* 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"); } + log("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); + /* load must have failed for some reason */ return -EINVAL; } -- cgit v1.2.3 From 2fbd23cf6ea535adccdeacfd12af731570524fd4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Sep 2013 17:31:58 +0200 Subject: sdlog2: position & velocity valid, postion global and landed flags added to LPOS, some refactoring --- .../position_estimator_inav/position_estimator_inav_main.c | 10 +++++----- src/modules/sdlog2/sdlog2.c | 3 +++ src/modules/sdlog2/sdlog2_messages.h | 5 ++++- src/modules/uORB/topics/vehicle_local_position.h | 4 ++-- 4 files changed, 14 insertions(+), 8 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 932f61088..10007bf96 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -259,7 +259,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.ref_timestamp = hrt_absolute_time(); local_pos.z_valid = true; local_pos.v_z_valid = true; - local_pos.global_z = true; + local_pos.z_global = true; } } } @@ -597,7 +597,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.timestamp = t; local_pos.xy_valid = can_estimate_xy && gps_valid; local_pos.v_xy_valid = can_estimate_xy; - local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented + local_pos.xy_global = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; @@ -610,9 +610,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); /* publish global position */ - global_pos.valid = local_pos.global_xy; + global_pos.valid = local_pos.xy_global; - if (local_pos.global_xy) { + if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = (int32_t)(est_lat * 1e7); @@ -630,7 +630,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.relative_alt = -local_pos.z; } - if (local_pos.global_z) { + if (local_pos.z_global) { global_pos.alt = local_pos.ref_alt - local_pos.z; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e83fb7dd3..ec8033202 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1046,6 +1046,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; + log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); + log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.landed = buf.local_pos.landed; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 1343bb3d0..0e88da054 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -109,6 +109,9 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; + uint8_t xy_flags; + uint8_t z_flags; + uint8_t landed; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -278,7 +281,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), + LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 31a0e632b..427153782 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -70,8 +70,8 @@ struct vehicle_local_position_s /* Heading */ float yaw; /* Reference position in GPS / WGS84 frame */ - bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ - bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ + bool xy_global; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ + bool z_global; /**< true if z is valid and has valid global reference (ref_alt) */ uint64_t ref_timestamp; /**< Time when reference position was set */ int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ -- cgit v1.2.3 From 2678aba9fe2a7f713dc48e8a362cd28d891b3675 Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 14 Sep 2013 21:41:35 -0700 Subject: A bit more NuttX gdb/python tooling to recover an interrupted context. Needs more fleshing out. --- Debug/Nuttx.py | 173 ++++++++++++++++++++++++++++++++++++++++----------------- 1 file changed, 123 insertions(+), 50 deletions(-) diff --git a/Debug/Nuttx.py b/Debug/Nuttx.py index b0864a229..093edd0e0 100644 --- a/Debug/Nuttx.py +++ b/Debug/Nuttx.py @@ -2,6 +2,106 @@ import gdb, gdb.types +class NX_register_set(object): + """Copy of the registers for a given context""" + + v7_regmap = { + 'R13': 0, + 'SP': 0, + 'PRIORITY': 1, + 'R4': 2, + 'R5': 3, + 'R6': 4, + 'R7': 5, + 'R8': 6, + 'R9': 7, + 'R10': 8, + 'R11': 9, + 'EXC_RETURN': 10, + 'R0': 11, + 'R1': 12, + 'R2': 13, + 'R3': 14, + 'R12': 15, + 'R14': 16, + 'LR': 16, + 'R15': 17, + 'PC': 17, + 'XPSR': 18, + } + + v7em_regmap = { + 'R13': 0, + 'SP': 0, + 'PRIORITY': 1, + 'R4': 2, + 'R5': 3, + 'R6': 4, + 'R7': 5, + 'R8': 6, + 'R9': 7, + 'R10': 8, + 'R11': 9, + 'EXC_RETURN': 10, + 'R0': 27, + 'R1': 28, + 'R2': 29, + 'R3': 30, + 'R12': 31, + 'R14': 32, + 'LR': 32, + 'R15': 33, + 'PC': 33, + 'XPSR': 34, + } + + regs = dict() + + def __init__(self, xcpt_regs): + if xcpt_regs is None: + self.regs['R0'] = long(gdb.parse_and_eval('$r0')) + self.regs['R1'] = long(gdb.parse_and_eval('$r1')) + self.regs['R2'] = long(gdb.parse_and_eval('$r2')) + self.regs['R3'] = long(gdb.parse_and_eval('$r3')) + self.regs['R4'] = long(gdb.parse_and_eval('$r4')) + self.regs['R5'] = long(gdb.parse_and_eval('$r5')) + self.regs['R6'] = long(gdb.parse_and_eval('$r6')) + self.regs['R7'] = long(gdb.parse_and_eval('$r7')) + self.regs['R8'] = long(gdb.parse_and_eval('$r8')) + self.regs['R9'] = long(gdb.parse_and_eval('$r9')) + self.regs['R10'] = long(gdb.parse_and_eval('$r10')) + self.regs['R11'] = long(gdb.parse_and_eval('$r11')) + self.regs['R12'] = long(gdb.parse_and_eval('$r12')) + self.regs['R13'] = long(gdb.parse_and_eval('$r13')) + self.regs['SP'] = long(gdb.parse_and_eval('$sp')) + self.regs['R14'] = long(gdb.parse_and_eval('$r14')) + self.regs['LR'] = long(gdb.parse_and_eval('$lr')) + self.regs['R15'] = long(gdb.parse_and_eval('$r15')) + self.regs['PC'] = long(gdb.parse_and_eval('$pc')) + self.regs['XPSR'] = long(gdb.parse_and_eval('$xpsr')) + else: + for key in self.v7em_regmap.keys(): + self.regs[key] = int(xcpt_regs[self.v7em_regmap[key]]) + + + @classmethod + def with_xcpt_regs(cls, xcpt_regs): + return cls(xcpt_regs) + + @classmethod + def for_current(cls): + return cls(None) + + def __format__(self, format_spec): + return format_spec.format( + registers = self.registers + ) + + @property + def registers(self): + return self.regs + + class NX_task(object): """Reference to a NuttX task and methods for introspecting it""" @@ -139,56 +239,12 @@ class NX_task(object): if 'registers' not in self.__dict__: registers = dict() if self._state_is('TSTATE_TASK_RUNNING'): - # XXX need to fix this to handle interrupt context - registers['R0'] = long(gdb.parse_and_eval('$r0')) - registers['R1'] = long(gdb.parse_and_eval('$r1')) - registers['R2'] = long(gdb.parse_and_eval('$r2')) - registers['R3'] = long(gdb.parse_and_eval('$r3')) - registers['R4'] = long(gdb.parse_and_eval('$r4')) - registers['R5'] = long(gdb.parse_and_eval('$r5')) - registers['R6'] = long(gdb.parse_and_eval('$r6')) - registers['R7'] = long(gdb.parse_and_eval('$r7')) - registers['R8'] = long(gdb.parse_and_eval('$r8')) - registers['R9'] = long(gdb.parse_and_eval('$r9')) - registers['R10'] = long(gdb.parse_and_eval('$r10')) - registers['R11'] = long(gdb.parse_and_eval('$r11')) - registers['R12'] = long(gdb.parse_and_eval('$r12')) - registers['R13'] = long(gdb.parse_and_eval('$r13')) - registers['SP'] = long(gdb.parse_and_eval('$sp')) - registers['R14'] = long(gdb.parse_and_eval('$r14')) - registers['LR'] = long(gdb.parse_and_eval('$lr')) - registers['R15'] = long(gdb.parse_and_eval('$r15')) - registers['PC'] = long(gdb.parse_and_eval('$pc')) - registers['XPSR'] = long(gdb.parse_and_eval('$xpsr')) - # this would only be valid if we were in an interrupt - registers['EXC_RETURN'] = 0 - # we should be able to get this... - registers['PRIMASK'] = 0 + registers = NX_register_set.for_current().registers else: context = self._tcb['xcp'] regs = context['regs'] - registers['R0'] = long(regs[27]) - registers['R1'] = long(regs[28]) - registers['R2'] = long(regs[29]) - registers['R3'] = long(regs[30]) - registers['R4'] = long(regs[2]) - registers['R5'] = long(regs[3]) - registers['R6'] = long(regs[4]) - registers['R7'] = long(regs[5]) - registers['R8'] = long(regs[6]) - registers['R9'] = long(regs[7]) - registers['R10'] = long(regs[8]) - registers['R11'] = long(regs[9]) - registers['R12'] = long(regs[31]) - registers['R13'] = long(regs[0]) - registers['SP'] = long(regs[0]) - registers['R14'] = long(regs[32]) - registers['LR'] = long(regs[32]) - registers['R15'] = long(regs[33]) - registers['PC'] = long(regs[33]) - registers['XPSR'] = long(regs[34]) - registers['EXC_RETURN'] = long(regs[10]) - registers['PRIMASK'] = long(regs[1]) + registers = NX_register_set.with_xcpt_regs(regs).registers + self.__dict__['registers'] = registers return self.__dict__['registers'] @@ -267,8 +323,7 @@ class NX_show_heap (gdb.Command): def _print_allocations(self, region_start, region_end): if region_start >= region_end: - print 'heap region {} corrupt'.format(hex(region_start)) - return + raise gdb.GdbError('heap region {} corrupt'.format(hex(region_start))) nodecount = region_end - region_start print 'heap {} - {}'.format(region_start, region_end) cursor = 1 @@ -286,13 +341,31 @@ class NX_show_heap (gdb.Command): nregions = heap['mm_nregions'] region_starts = heap['mm_heapstart'] region_ends = heap['mm_heapend'] - print "{} heap(s)".format(nregions) + print '{} heap(s)'.format(nregions) # walk the heaps for i in range(0, nregions): self._print_allocations(region_starts[i], region_ends[i]) NX_show_heap() +class NX_show_interrupted_thread (gdb.Command): + """(NuttX) prints the register state of an interrupted thread when in interrupt/exception context""" + def __init__(self): + super(NX_show_interrupted_thread, self).__init__('show interrupted-thread', gdb.COMMAND_USER) + def invoke(self, args, from_tty): + regs = gdb.lookup_global_symbol('current_regs').value() + if regs is 0: + raise gdb.GdbError('not in interrupt context') + else: + registers = NX_register_set.with_xcpt_regs(regs) + my_fmt = '' + my_fmt += ' R0 {registers[R0]:#010x} {registers[R1]:#010x} {registers[R2]:#010x} {registers[R3]:#010x}\n' + my_fmt += ' R4 {registers[R4]:#010x} {registers[R5]:#010x} {registers[R6]:#010x} {registers[R7]:#010x}\n' + my_fmt += ' R8 {registers[R8]:#010x} {registers[R9]:#010x} {registers[R10]:#010x} {registers[R11]:#010x}\n' + my_fmt += ' R12 {registers[PC]:#010x}\n' + my_fmt += ' SP {registers[SP]:#010x} LR {registers[LR]:#010x} PC {registers[PC]:#010x} XPSR {registers[XPSR]:#010x}\n' + print format(registers, my_fmt) +NX_show_interrupted_thread() -- cgit v1.2.3 From 03727974f1249adce8f55faf4978910699e5a47e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 15 Sep 2013 18:48:28 +0200 Subject: Fix binding states for DSM --- src/drivers/px4io/px4io.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 3e83fcad8..5fff1feac 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -853,9 +853,9 @@ PX4IO::task_main() int32_t dsm_bind_val; param_t dsm_bind_param; - // See if bind parameter has been set, and reset it to 0 + // See if bind parameter has been set, and reset it to -1 param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); - if (dsm_bind_val) { + if (dsm_bind_val >= 0) { dsm_bind_ioctl(dsm_bind_val); dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); @@ -1167,7 +1167,7 @@ PX4IO::io_handle_status(uint16_t status) void PX4IO::dsm_bind_ioctl(int dsmMode) { - if (!(_status & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { + if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { /* 0: dsm2, 1:dsmx */ if ((dsmMode >= 0) && (dsmMode <= 1)) { mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); -- cgit v1.2.3 From 1a641b791dd3802571e56521b7d13585c07061ef Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 16 Sep 2013 11:33:29 +0900 Subject: tone_alarm: more device paths replaced with #define --- src/modules/commander/commander_helper.cpp | 2 +- src/systemcmds/preflight_check/preflight_check.c | 2 +- src/systemcmds/tests/test_hrt.c | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 7feace2b4..017499cb5 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -82,7 +82,7 @@ static int buzzer; int buzzer_init() { - buzzer = open("/dev/tone_alarm", O_WRONLY); + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { warnx("Buzzer: open fail\n"); diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index f5bd01ce8..e9c5f1a2c 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -163,7 +163,7 @@ system_eval: warnx("PREFLIGHT CHECK ERROR! TRIGGERING ALARM"); fflush(stderr); - int buzzer = open("/dev/tone_alarm", O_WRONLY); + int buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); int leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { diff --git a/src/systemcmds/tests/test_hrt.c b/src/systemcmds/tests/test_hrt.c index ba6b86adb..5690997a9 100644 --- a/src/systemcmds/tests/test_hrt.c +++ b/src/systemcmds/tests/test_hrt.c @@ -124,10 +124,10 @@ int test_tone(int argc, char *argv[]) int fd, result; unsigned long tone; - fd = open("/dev/tone_alarm", O_WRONLY); + fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (fd < 0) { - printf("failed opening /dev/tone_alarm\n"); + printf("failed opening " TONEALARM_DEVICE_PATH "\n"); goto out; } -- cgit v1.2.3 From f4abcb51a1a40cccf8f929fe1598297ea1d07c4b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 15 Sep 2013 17:16:47 +0900 Subject: tone_alarm: add #define for device path --- src/drivers/drv_tone_alarm.h | 2 ++ src/drivers/stm32/tone_alarm/tone_alarm.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index f0b860620..caf2b0f6e 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -60,6 +60,8 @@ #include +#define TONEALARM_DEVICE_PATH "/dev/tone_alarm" + #define _TONE_ALARM_BASE 0x7400 #define TONE_SET_ALARM _IOC(_TONE_ALARM_BASE, 1) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a582ece17..930809036 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -317,7 +317,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : - CDev("tone_alarm", "/dev/tone_alarm"), + CDev("tone_alarm", TONEALARM_DEVICE_PATH), _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), @@ -820,10 +820,10 @@ play_tune(unsigned tune) { int fd, ret; - fd = open("/dev/tone_alarm", 0); + fd = open(TONEALARM_DEVICE_PATH, 0); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = ioctl(fd, TONE_SET_ALARM, tune); close(fd); @@ -839,10 +839,10 @@ play_string(const char *str, bool free_buffer) { int fd, ret; - fd = open("/dev/tone_alarm", O_WRONLY); + fd = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (fd < 0) - err(1, "/dev/tone_alarm"); + err(1, TONEALARM_DEVICE_PATH); ret = write(fd, str, strlen(str) + 1); close(fd); -- cgit v1.2.3