diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-05 17:56:10 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2012-11-05 17:56:10 +0100 |
commit | 572084f3571173650fad355b366ebfd598afd303 (patch) | |
tree | a0d7c88780d98f5081f3bec50c1828925c8878fd | |
parent | 904efa8fa87c09a2e7f18f0821431e75eb13e67b (diff) | |
parent | e7f2c053c241849e3ea035fcd22a0e29945b3415 (diff) | |
download | px4-firmware-572084f3571173650fad355b366ebfd598afd303.tar.gz px4-firmware-572084f3571173650fad355b366ebfd598afd303.tar.bz2 px4-firmware-572084f3571173650fad355b366ebfd598afd303.zip |
Merge branch 'master' of https://github.com/PX4/Firmware into fw_control
59 files changed, 1273 insertions, 2930 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index e2478b4ec..9dbdf49e2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -143,7 +143,7 @@ int commander_thread_main(int argc, char *argv[]); static int buzzer_init(void); static void buzzer_deinit(void); -static void tune_confirm(); +static void tune_confirm(void); static int led_init(void); static void led_deinit(void); static int led_toggle(int led); @@ -264,7 +264,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u return 0; } -void tune_confirm() { +void tune_confirm(void) { ioctl(buzzer, TONE_SET_ALARM, 3); } @@ -914,17 +914,69 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } break; - /* - * do not report an error for commands that are - * handled directly by MAVLink. - */ - case MAV_CMD_PREFLIGHT_STORAGE: + case MAV_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed) { + mavlink_log_info(mavlink_fd, "[cmd] REJECTING param command while armed"); + } else { + + // XXX move this to LOW PRIO THREAD of commander app + /* Read all parameters from EEPROM to RAM */ + + if (((int)(cmd->param1)) == 0) { + + /* read all parameters from EEPROM to RAM */ + int read_ret = param_load_default(); + if (read_ret == OK) { + //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "[cmd] OK loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = MAV_RESULT_ACCEPTED; + } else if (read_ret == 1) { + mavlink_log_info(mavlink_fd, "[cmd] OK no changes in"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = MAV_RESULT_ACCEPTED; + } else { + if (read_ret < -1) { + mavlink_log_info(mavlink_fd, "[cmd] ERR loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } else { + mavlink_log_info(mavlink_fd, "[cmd] ERR no param file named"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + result = MAV_RESULT_FAILED; + } + + } else if (((int)(cmd->param1)) == 1) { + + /* write all parameters from RAM to EEPROM */ + int write_ret = param_save_default(); + if (write_ret == OK) { + mavlink_log_info(mavlink_fd, "[cmd] OK saved param file"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = MAV_RESULT_ACCEPTED; + + } else { + if (write_ret < -1) { + mavlink_log_info(mavlink_fd, "[cmd] ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } else { + mavlink_log_info(mavlink_fd, "[cmd] ERR writing params to"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + result = MAV_RESULT_FAILED; + } + + } else { + mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = MAV_RESULT_UNSUPPORTED; + } + } + } break; default: { - mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command"); + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); result = MAV_RESULT_UNSUPPORTED; - usleep(200000); /* announce command rejection */ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -932,7 +984,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } /* supported command handling stop */ - + if (result == MAV_RESULT_FAILED || + result == MAV_RESULT_DENIED || + result == MAV_RESULT_UNSUPPORTED) { + ioctl(buzzer, TONE_SET_ALARM, 5); + } else if (result == MAV_RESULT_ACCEPTED) { + tune_confirm(); + } /* send any requested ACKs */ if (cmd->confirmation > 0) { @@ -1523,12 +1581,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_control_manual_enabled = false; current_status.flag_control_offboard_enabled = true; state_changed = true; + tune_confirm(); - mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST TIME."); + mavlink_log_critical(mavlink_fd, "[commander] DETECTED OFFBOARD CONTROL SIGNAL FIRST"); } else { if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "[commander] RECOVERY - OFFBOARD CONTROL SIGNAL GAINED!"); + mavlink_log_critical(mavlink_fd, "[commander] OK:RECOVERY OFFBOARD CONTROL"); state_changed = true; + tune_confirm(); } } @@ -1550,7 +1610,7 @@ int commander_thread_main(int argc, char *argv[]) /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { current_status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "[commander] CRITICAL - NO OFFBOARD CONTROL SIGNAL!"); + mavlink_log_critical(mavlink_fd, "[commander] CRIT:NO OFFBOARD CONTROL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ @@ -1560,10 +1620,11 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost_interval > 100000) { current_status.offboard_control_signal_lost = true; current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - current_status.failsave_lowlevel = true; + tune_confirm(); /* kill motors after timeout */ if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms*1000) { + current_status.failsave_lowlevel = true; state_changed = true; } } diff --git a/apps/drivers/boards/px4fmu/Makefile b/apps/drivers/boards/px4fmu/Makefile index 393e96e32..6b183d8d2 100644 --- a/apps/drivers/boards/px4fmu/Makefile +++ b/apps/drivers/boards/px4fmu/Makefile @@ -36,5 +36,6 @@ # INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common +LIBNAME = brd_px4fmu include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/boards/px4fmu/px4fmu_init.c b/apps/drivers/boards/px4fmu/px4fmu_init.c index 568d861c9..57ffb77d3 100644 --- a/apps/drivers/boards/px4fmu/px4fmu_init.c +++ b/apps/drivers/boards/px4fmu/px4fmu_init.c @@ -57,7 +57,6 @@ #include <nuttx/i2c.h> #include <nuttx/mmcsd.h> #include <nuttx/analog/adc.h> -#include <nuttx/arch.h> #include "stm32_internal.h" #include "px4fmu_internal.h" diff --git a/apps/drivers/boards/px4io/Makefile b/apps/drivers/boards/px4io/Makefile new file mode 100644 index 000000000..85806fe6f --- /dev/null +++ b/apps/drivers/boards/px4io/Makefile @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Board-specific startup code for the PX4IO +# + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common +LIBNAME = brd_px4io + +include $(APPDIR)/mk/app.mk diff --git a/nuttx/configs/px4io/src/up_boot.c b/apps/drivers/boards/px4io/px4io_init.c index 9d6a3b246..33a6707ce 100644 --- a/nuttx/configs/px4io/src/up_boot.c +++ b/apps/drivers/boards/px4io/px4io_init.c @@ -1,82 +1,95 @@ -/************************************************************************************
- * configs/stm3210e-eval/src/up_boot.c
- * arch/arm/src/board/up_boot.c
- *
- * Copyright (C) 2009 Gregory Nutt. All rights reserved.
- * Author: Gregory Nutt <spudmonkey@racsa.co.cr>
- *
- * 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 NuttX 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.
- *
- ************************************************************************************/
-
-/************************************************************************************
- * Included Files
- ************************************************************************************/
-
-#include <nuttx/config.h>
-
-#include <debug.h>
-
-#include <arch/board/board.h>
-
-#include "up_arch.h"
-#include "px4io_internal.h"
-#include <arch/board/up_hrt.h>
-#include <arch/board/drv_pwm_servo.h>
-
-/************************************************************************************
- * Definitions
- ************************************************************************************/
-
-/************************************************************************************
- * Private Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Public Functions
- ************************************************************************************/
-
-/************************************************************************************
- * Name: stm32_boardinitialize
- *
- * Description:
- * All STM32 architectures must provide the following entry point. This entry point
- * is called early in the intitialization -- after all memory has been configured
- * and mapped but before any devices have been initialized.
- *
- ************************************************************************************/
-
-void stm32_boardinitialize(void)
-{
- /* Configure on-board LEDs if LED support has been selected. */
-
-#ifdef CONFIG_ARCH_LEDS
- up_ledinit();
-#endif
-
-}
+/**************************************************************************** + * + * Copyright (C) 2012 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 px4io_init.c + * + * PX4IO-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdio.h> +#include <debug.h> +#include <errno.h> + +#include <nuttx/arch.h> + +#include "stm32_internal.h" +#include "px4io_internal.h" +#include "stm32_uart.h" + +#include <arch/board/board.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_led.h> +#include <drivers/drv_pwm_output.h> + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void stm32_boardinitialize(void) +{ + /* configure GPIOs */ + stm32_configgpio(GPIO_ACC1_PWR_EN); + stm32_configgpio(GPIO_ACC2_PWR_EN); + stm32_configgpio(GPIO_SERVO_PWR_EN); + stm32_configgpio(GPIO_RELAY1_EN); + stm32_configgpio(GPIO_RELAY2_EN); + stm32_configgpio(GPIO_LED1); + stm32_configgpio(GPIO_LED2); + stm32_configgpio(GPIO_LED3); + stm32_configgpio(GPIO_ACC_OC_DETECT); + stm32_configgpio(GPIO_SERVO_OC_DETECT); + stm32_configgpio(GPIO_BTN_SAFETY); +} diff --git a/nuttx/configs/px4io/src/px4io_internal.h b/apps/drivers/boards/px4io/px4io_internal.h index 877a06653..a0342ac8a 100644 --- a/nuttx/configs/px4io/src/px4io_internal.h +++ b/apps/drivers/boards/px4io/px4io_internal.h @@ -35,8 +35,7 @@ * @file PX4IO hardware definitions. */ -#ifndef __CONFIGS_PX4IO_SRC_PX4IO_INTERNAL_H -#define __CONFIGS_PX4IO_SRC_PX4IO_INTERNAL_H +#pragma once /************************************************************************************ * Included Files @@ -46,6 +45,8 @@ #include <nuttx/compiler.h> #include <stdint.h> +#include <stm32_internal.h> + /************************************************************************************ * Definitions ************************************************************************************/ @@ -97,21 +98,3 @@ #define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13) #define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12) - -/************************************************************************************ - * Public Types - ************************************************************************************/ - -/************************************************************************************ - * Public data - ************************************************************************************/ - -#ifndef __ASSEMBLY__ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -#endif /* __ASSEMBLY__ */ -#endif /* __CONFIGS_PX4IO_SRC_PX4IO_INTERNAL_H */ - diff --git a/apps/drivers/boards/px4io/px4io_pwm_servo.c b/apps/drivers/boards/px4io/px4io_pwm_servo.c new file mode 100644 index 000000000..a2f73c429 --- /dev/null +++ b/apps/drivers/boards/px4io/px4io_pwm_servo.c @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 px4fmu_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include <stdint.h> + +#include <drivers/stm32/drv_pwm_servo.h> + +#include <arch/board/board.h> +#include <drivers/drv_pwm_output.h> + +#include <stm32_internal.h> +#include <stm32_gpio.h> +#include <stm32_tim.h> + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM2_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM2EN, + .clock_freq = STM32_APB1_TIM2_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + }, + { + .base = STM32_TIM4_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM4EN, + .clock_freq = STM32_APB1_TIM4_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM2_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM2_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH3OUT, + .timer_index = 2, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM4_CH4OUT, + .timer_index = 2, + .timer_channel = 4, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1000, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1000, + } +}; diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h index b8d1fa0f0..92d184326 100644 --- a/apps/drivers/drv_gpio.h +++ b/apps/drivers/drv_gpio.h @@ -67,6 +67,38 @@ #endif +#ifdef CONFIG_ARCH_BOARD_PX4IO +/* + * PX4IO GPIO numbers. + * + * XXX note that these are here for reference/future use; currently + * there is no good way to wire these up without a common STM32 GPIO + * driver, which isn't implemented yet. + */ +/* outputs */ +# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */ +# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */ +# define GPIO_SERVO_POWER (1<<2) /**< servo power */ +# define GPIO_RELAY1 (1<<3) /**< relay 1 */ +# define GPIO_RELAY2 (1<<4) /**< relay 2 */ +# define GPIO_LED_BLUE (1<<5) /**< blue LED */ +# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */ +# define GPIO_LED_SAFETY (1<<7) /**< safety LED */ + +/* inputs */ +# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */ +# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */ +# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */ + +/** + * Default GPIO device - other devices may also support this protocol if + * they also export GPIO-like things. This is always the GPIOs on the + * main board. + */ +# define GPIO_DEVICE_PATH "/dev/px4io" + +#endif + #ifndef GPIO_DEVICE_PATH # error No GPIO support for this board. #endif diff --git a/apps/drivers/drv_led.h b/apps/drivers/drv_led.h index bf21787f2..7eb9e4b7c 100644 --- a/apps/drivers/drv_led.h +++ b/apps/drivers/drv_led.h @@ -50,6 +50,7 @@ #define LED_AMBER 0 #define LED_RED 0 /* some boards have red rather than amber */ #define LED_BLUE 1 +#define LED_SAFETY 2 #define LED_ON _IOC(_LED_BASE, 0) #define LED_OFF _IOC(_LED_BASE, 1) @@ -59,6 +60,6 @@ __BEGIN_DECLS /* * Initialise the LED driver. */ -__EXPORT extern void drv_led_start(); +__EXPORT extern void drv_led_start(void); __END_DECLS diff --git a/apps/drivers/drv_rc_input.h b/apps/drivers/drv_rc_input.h index 532e95fb5..927406108 100644 --- a/apps/drivers/drv_rc_input.h +++ b/apps/drivers/drv_rc_input.h @@ -57,15 +57,23 @@ #define RC_INPUT_DEVICE_PATH "/dev/input_rc" /** - * Maximum number of R/C input channels in the system. + * Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. */ -#define RC_INPUT_MAX_CHANNELS 16 +#define RC_INPUT_MAX_CHANNELS 18 /** * Input signal type, value is a control position from zero to 100 * percent. */ -typedef uint8_t rc_input_t; +typedef uint16_t rc_input_t; + +enum RC_INPUT_SOURCE { + RC_INPUT_SOURCE_UNKNOWN = 0, + RC_INPUT_SOURCE_PX4FMU_PPM, + RC_INPUT_SOURCE_PX4IO_PPM, + RC_INPUT_SOURCE_PX4IO_SPEKTRUM, + RC_INPUT_SOURCE_PX4IO_SBUS +}; /** * R/C input status structure. @@ -74,10 +82,16 @@ typedef uint8_t rc_input_t; * on the board involved. */ struct rc_input_values { + /** decoding time */ + uint64_t timestamp; + /** number of channels actually being seen */ uint32_t channel_count; - /** desired pulse widths for each of the supported channels */ + /** Input source */ + enum RC_INPUT_SOURCE input_source; + + /** measured pulse widths for each of the supported channels */ rc_input_t values[RC_INPUT_MAX_CHANNELS]; }; diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 2b4fab151..81bc8954b 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -66,6 +66,9 @@ #include <drivers/drv_mag.h> #include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/subsystem_info.h> + #include <float.h> /* @@ -631,6 +634,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCSSCALE: /* set new scale factors */ memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); + (void)check_calibration(); return 0; case MAGIOCGSCALE: @@ -1039,11 +1043,17 @@ int HMC5883::check_calibration() offset_valid = false; } - if (_calibrated && !(offset_valid && scale_valid)) { - warnx("warning: mag %s%s", (scale_valid) ? "" : "scale invalid. ", + if (_calibrated != (offset_valid && scale_valid)) { + warnx("warning: mag cal changed: %s%s", (scale_valid) ? "" : "scale invalid. ", (offset_valid) ? "" : "offset invalid."); - _calibrated = false; - // XXX Notify system via uORB + _calibrated = (offset_valid && scale_valid); + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + _calibrated, + SUBSYSTEM_TYPE_MAG}; + orb_advert_t pub = orb_advertise(ORB_ID(subsystem_info), &info); } return 0; } diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 995c9393f..4687df2aa 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -61,9 +61,10 @@ #include <drivers/device/device.h> #include <drivers/drv_rc_input.h> #include <drivers/drv_pwm_output.h> -#include <systemlib/mixer/mixer.h> +#include <drivers/drv_hrt.h> #include <drivers/drv_mixer.h> +#include <systemlib/mixer/mixer.h> #include <systemlib/perf_counter.h> #include <systemlib/hx_stream.h> #include <systemlib/err.h> @@ -73,7 +74,7 @@ #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/rc_channels.h> -#include "px4io/protocol.h" +#include <px4io/protocol.h> #include "uploader.h" @@ -87,6 +88,8 @@ public: virtual int ioctl(file *filp, int cmd, unsigned long arg); + void set_rx_mode(unsigned mode); + private: static const unsigned _max_actuators = PX4IO_OUTPUT_CHANNELS; @@ -95,6 +98,7 @@ private: int _task; ///< worker task volatile bool _task_should_exit; + volatile bool _connected; ///< true once we have received a valid frame int _t_actuators; ///< actuator output topic actuator_controls_s _controls; ///< actuator outputs @@ -102,6 +106,9 @@ private: int _t_armed; ///< system armed control topic actuator_armed_s _armed; ///< system armed state + orb_advert_t _to_input_rc; ///< rc inputs from io + rc_input_values _input_rc; ///< rc input values + orb_advert_t _t_outputs; ///< mixed outputs topic actuator_outputs_s _outputs; ///< mixed outputs @@ -113,6 +120,9 @@ private: // XXX how should this work? bool _send_needed; ///< If true, we need to send a packet to IO + bool _config_needed; ///< if true, we need to set a config update to IO + + uint8_t _rx_mode; ///< the current RX mode on IO /** * Trampoline to the worker task @@ -145,6 +155,11 @@ private: void io_send(); /** + * Send a config packet to PX4IO + */ + void config_send(); + + /** * Mixer control callback; invoked to fetch a control from a specific * group/index during mixing. */ @@ -168,13 +183,16 @@ PX4IO::PX4IO() : _io_stream(nullptr), _task(-1), _task_should_exit(false), + _connected(false), _t_actuators(-1), _t_armed(-1), _t_outputs(-1), _mixers(nullptr), _primary_pwm_device(false), _switch_armed(false), - _send_needed(false) + _send_needed(false), + _config_needed(false), + _rx_mode(RX_MODE_PPM_ONLY) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -238,12 +256,24 @@ PX4IO::init() /* start the IO interface task */ _task = task_create("px4io", SCHED_PRIORITY_DEFAULT, 4096, (main_t)&PX4IO::task_main_trampoline, nullptr); - if (_task < 0) { debug("task start failed: %d", errno); return -errno; } + /* wait a second for it to detect IO */ + for (unsigned i = 0; i < 10; i++) { + if (_connected) { + debug("PX4IO connected"); + break; + } + usleep(100000); + } + if (!_connected) { + /* error here will result in everything being torn down */ + log("PX4IO not responding"); + return -EIO; + } return OK; } @@ -262,22 +292,20 @@ PX4IO::task_main() _serial_fd = ::open("/dev/ttyS2", O_RDWR); if (_serial_fd < 0) { - debug("failed to open serial port for IO: %d", errno); - _task = -1; - _exit(errno); + log("failed to open serial port: %d", errno); + goto out; } /* protocol stream */ _io_stream = hx_stream_init(_serial_fd, &PX4IO::rx_callback_trampoline, this); - - perf_counter_t pc_tx_frames = perf_alloc(PC_COUNT, "PX4IO frames transmitted"); - perf_counter_t pc_rx_frames = perf_alloc(PC_COUNT, "PX4IO frames received"); - perf_counter_t pc_rx_errors = perf_alloc(PC_COUNT, "PX4IO receive errors"); - hx_stream_set_counters(_io_stream, pc_tx_frames, pc_rx_frames, pc_rx_errors); - - /* XXX send a who-are-you request */ - - /* XXX verify firmware/protocol version */ + if (_io_stream == nullptr) { + log("failed to allocate HX protocol stream"); + goto out; + } + hx_stream_set_counters(_io_stream, + perf_alloc(PC_COUNT, "PX4IO frames transmitted"), + perf_alloc(PC_COUNT, "PX4IO frames received"), + perf_alloc(PC_COUNT, "PX4IO receive errors")); /* * Subscribe to the appropriate PWM output topic based on whether we are the @@ -293,9 +321,14 @@ PX4IO::task_main() orb_set_interval(_t_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ + memset(&_outputs, 0, sizeof(_outputs)); _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &_outputs); + /* advertise the rc inputs */ + memset(&_input_rc, 0, sizeof(_input_rc)); + _to_input_rc = orb_advertise(ORB_ID(input_rc), &_input_rc); + /* poll descriptor */ pollfd fds[3]; fds[0].fd = _serial_fd; @@ -311,7 +344,7 @@ PX4IO::task_main() while (!_task_should_exit) { /* sleep waiting for data, but no more than 100ms */ - int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 1000); + int ret = ::poll(&fds[0], sizeof(fds) / sizeof(fds[0]), 100); /* this would be bad... */ if (ret < 0) { @@ -361,8 +394,18 @@ PX4IO::task_main() _send_needed = false; io_send(); } + + /* send a config packet to IO if required */ + if (_config_needed) { + _config_needed = false; + config_send(); + } } +out: + if (_io_stream != nullptr) + hx_stream_free(_io_stream); + /* tell the dtor that we are exiting */ _task = -1; _exit(0); @@ -409,15 +452,34 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received) { const px4io_report *rep = (const px4io_report *)buffer; + lock(); + /* sanity-check the received frame size */ - if (bytes_received != sizeof(px4io_report)) - return; + if (bytes_received != sizeof(px4io_report)) { + debug("got %u expected %u", bytes_received, sizeof(px4io_report)); + goto out; + } + if (rep->i2f_magic != I2F_MAGIC) { + debug("bad magic"); + goto out; + } + _connected = true; + + /* publish raw rc channel values from IO */ + _input_rc.timestamp = hrt_absolute_time(); + for (int i = 0; i < rep->channel_count; i++) + { + _input_rc.values[i] = rep->rc_channel[i]; + } - /* XXX handle R/C inputs here ... needs code sharing/library */ + orb_publish(ORB_ID(input_rc), _to_input_rc, &_input_rc); /* remember the latched arming switch state */ _switch_armed = rep->armed; + _send_needed = true; + +out: unlock(); } @@ -425,6 +487,7 @@ void PX4IO::io_send() { px4io_command cmd; + int ret; cmd.f2i_magic = F2I_MAGIC; @@ -439,7 +502,23 @@ PX4IO::io_send() cmd.arm_ok = _armed.armed; - hx_stream_send(_io_stream, &cmd, sizeof(cmd)); + ret = hx_stream_send(_io_stream, &cmd, sizeof(cmd)); + if (ret) + debug("send error %d", ret); +} + +void +PX4IO::config_send() +{ + px4io_config cfg; + int ret; + + cfg.f2i_config_magic = F2I_CONFIG_MAGIC; + cfg.serial_rx_mode = _rx_mode; + + ret = hx_stream_send(_io_stream, &cfg, sizeof(cfg)); + if (ret) + debug("config error %d", ret); } int @@ -555,8 +634,49 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) return ret; } +void +PX4IO::set_rx_mode(unsigned mode) +{ + if (mode != _rx_mode) { + _rx_mode = mode; + _config_needed = true; + } +} + extern "C" __EXPORT int px4io_main(int argc, char *argv[]); +namespace +{ + +void +test(void) +{ + int fd; + + fd = open(PWM_OUTPUT_DEVICE_PATH, 0); + + if (fd < 0) { + puts("open fail"); + exit(1); + } + + ioctl(fd, PWM_SERVO_ARM, 0); + ioctl(fd, PWM_SERVO_SET(0), 1000); + ioctl(fd, PWM_SERVO_SET(1), 1100); + ioctl(fd, PWM_SERVO_SET(2), 1200); + ioctl(fd, PWM_SERVO_SET(3), 1300); + ioctl(fd, PWM_SERVO_SET(4), 1400); + ioctl(fd, PWM_SERVO_SET(5), 1500); + ioctl(fd, PWM_SERVO_SET(6), 1600); + ioctl(fd, PWM_SERVO_SET(7), 1700); + + close(fd); + + exit(0); +} + +} + int px4io_main(int argc, char *argv[]) { @@ -624,5 +744,25 @@ px4io_main(int argc, char *argv[]) return ret; } - errx(1, "need a verb, only support 'start' and 'update'"); + if (!strcmp(argv[1], "rx_spektrum6")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_6); + } + if (!strcmp(argv[1], "rx_spektrum7")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_SPEKTRUM_7); + } + if (!strcmp(argv[1], "rx_sbus")) { + if (g_dev == nullptr) + errx(1, "not started"); + g_dev->set_rx_mode(RX_MODE_FUTABA_SBUS); + } + + if (!strcmp(argv[1], "test")) + test(); + + + errx(1, "need a command, try 'start', 'test', 'rx_spektrum6', 'rx_spektrum7', 'rx_sbus' or 'update'"); } diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index 1ac90b16d..cd9cb45b1 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -276,6 +276,17 @@ static void hrt_call_invoke(void); * Specific registers and bits used by PPM sub-functions */ #ifdef CONFIG_HRT_PPM +/* + * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. + */ +# ifndef GTIM_CCER_CC1NP +# define GTIM_CCER_CC1NP 0 +# define GTIM_CCER_CC2NP 0 +# define GTIM_CCER_CC3NP 0 +# define GTIM_CCER_CC4NP 0 +# define PPM_EDGE_FLIP +# endif + # if HRT_PPM_CHANNEL == 1 # define rCCR_PPM rCCR1 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ @@ -284,6 +295,7 @@ static void hrt_call_invoke(void); # define CCMR1_PPM 1 /* not on TI1/TI2 */ # define CCMR2_PPM 0 /* on TI3, not on TI4 */ # define CCER_PPM (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC1P # elif HRT_PPM_CHANNEL == 2 # define rCCR_PPM rCCR2 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC2IE /* capture interrupt (non-DMA mode) */ @@ -292,6 +304,7 @@ static void hrt_call_invoke(void); # define CCMR1_PPM 2 /* not on TI1/TI2 */ # define CCMR2_PPM 0 /* on TI3, not on TI4 */ # define CCER_PPM (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC2P # elif HRT_PPM_CHANNEL == 3 # define rCCR_PPM rCCR3 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC3IE /* capture interrupt (non-DMA mode) */ @@ -300,6 +313,7 @@ static void hrt_call_invoke(void); # define CCMR1_PPM 0 /* not on TI1/TI2 */ # define CCMR2_PPM 1 /* on TI3, not on TI4 */ # define CCER_PPM (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC3P # elif HRT_PPM_CHANNEL == 4 # define rCCR_PPM rCCR4 /* capture register for PPM */ # define DIER_PPM GTIM_DIER_CC4IE /* capture interrupt (non-DMA mode) */ @@ -308,6 +322,7 @@ static void hrt_call_invoke(void); # define CCMR1_PPM 0 /* not on TI1/TI2 */ # define CCMR2_PPM 2 /* on TI3, not on TI4 */ # define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */ +# define CCER_PPM_FLIP GTIM_CCER_CC4P # else # error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set # endif @@ -323,7 +338,7 @@ static void hrt_call_invoke(void); /* decoded PPM buffer */ #define PPM_MAX_CHANNELS 12 __EXPORT uint16_t ppm_buffer[PPM_MAX_CHANNELS]; -__EXPORT unsigned ppm_decoded_channels; +__EXPORT unsigned ppm_decoded_channels = 0; __EXPORT uint64_t ppm_last_valid_decode = 0; /* PPM edge history */ @@ -371,12 +386,12 @@ static void hrt_ppm_decode(uint32_t status); static void hrt_tim_init(void) { - /* clock/power on our timer */ - modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); - /* claim our interrupt vector */ irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); + /* clock/power on our timer */ + modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); + /* disable and configure the timer */ rCR1 = 0; rCR2 = 0; @@ -532,9 +547,14 @@ hrt_tim_isr(int irq, void *context) #ifdef CONFIG_HRT_PPM /* was this a PPM edge? */ - if (status & (SR_INT_PPM | SR_OVF_PPM)) - hrt_ppm_decode(status); + if (status & (SR_INT_PPM | SR_OVF_PPM)) { + /* if required, flip edge sensitivity */ +# ifdef PPM_EDGE_FLIP + rCCER ^= CCER_PPM_FLIP; +# endif + hrt_ppm_decode(status); + } #endif /* was this a timer tick? */ @@ -562,7 +582,7 @@ hrt_absolute_time(void) { hrt_abstime abstime; uint32_t count; - uint32_t flags; + irqstate_t flags; /* * Counter state. Marked volatile as they may change diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index e3801a417..50aa34d81 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -171,10 +171,8 @@ pwm_channel_init(unsigned channel) int up_pwm_servo_set(unsigned channel, servo_position_t value) { - if (channel >= PWM_SERVO_MAX_CHANNELS) { - lldbg("pwm_channel_set: bogus channel %u\n", channel); + if (channel >= PWM_SERVO_MAX_CHANNELS) return -1; - } unsigned timer = pwm_channels[channel].timer_index; @@ -214,17 +212,15 @@ up_pwm_servo_set(unsigned channel, servo_position_t value) servo_position_t up_pwm_servo_get(unsigned channel) { - if (channel >= PWM_SERVO_MAX_CHANNELS) { - lldbg("pwm_channel_get: bogus channel %u\n", channel); + if (channel >= PWM_SERVO_MAX_CHANNELS) return 0; - } unsigned timer = pwm_channels[channel].timer_index; servo_position_t value = 0; /* test timer for validity */ if ((pwm_timers[timer].base == 0) || - (pwm_channels[channel].gpio == 0)) + (pwm_channels[channel].timer_channel == 0)) return 0; /* configure the channel */ @@ -246,7 +242,7 @@ up_pwm_servo_get(unsigned channel) break; } - return value; + return value + 1; } int @@ -261,7 +257,7 @@ up_pwm_servo_init(uint32_t channel_mask) /* now init channels */ for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { /* don't do init for disabled channels; this leaves the pin configs alone */ - if (((1 << i) & channel_mask) && (pwm_channels[i].gpio != 0)) + if (((1 << i) & channel_mask) && (pwm_channels[i].timer_channel != 0)) pwm_channel_init(i); } diff --git a/apps/drivers/stm32/drv_pwm_servo.h b/apps/drivers/stm32/drv_pwm_servo.h index 667283424..5dd4cf70c 100644 --- a/apps/drivers/stm32/drv_pwm_servo.h +++ b/apps/drivers/stm32/drv_pwm_servo.h @@ -42,7 +42,7 @@ #include <drivers/drv_pwm_output.h> /* configuration limits */ -#define PWM_SERVO_MAX_TIMERS 2 +#define PWM_SERVO_MAX_TIMERS 4 #define PWM_SERVO_MAX_CHANNELS 8 /* array of timers dedicated to PWM servo use */ @@ -53,9 +53,6 @@ struct pwm_servo_timer { uint32_t clock_freq; }; -/* supplied by board-specific code */ -__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS]; - /* array of channels in logical order */ struct pwm_servo_channel { uint32_t gpio; @@ -64,4 +61,6 @@ struct pwm_servo_channel { servo_position_t default_value; }; +/* supplied by board-specific code */ +__EXPORT extern const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS]; __EXPORT extern const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS]; diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7d8232b3a..9b2cfcbba 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -589,16 +589,15 @@ int mavlink_thread_main(int argc, char *argv[]) /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { - /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200); /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); - /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + /* 5 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); - /* 1 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300); diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 6d434ed3d..7cb1582b4 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -233,66 +233,68 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } break; - case MAVLINK_MSG_ID_COMMAND_LONG: { - mavlink_command_long_t cmd_mavlink; - mavlink_msg_command_long_decode(msg, &cmd_mavlink); - - uint8_t result = MAV_RESULT_UNSUPPORTED; - - if (cmd_mavlink.target_system == mavlink_system.sysid && - ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { - - /* preflight parameter load / store */ - if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) { - /* Read all parameters from EEPROM to RAM */ - - if (((int)(cmd_mavlink.param1)) == 0) { - - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - if (read_ret == OK) { - //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params"); - result = MAV_RESULT_ACCEPTED; - } else if (read_ret == 1) { - mavlink_missionlib_send_gcs_string("[mavlink pm] No stored parameters to load"); - result = MAV_RESULT_ACCEPTED; - } else { - if (read_ret < -1) { - mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params from EEPROM"); - } else { - mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params, no EEPROM found"); - } - result = MAV_RESULT_FAILED; - } - - } else if (((int)(cmd_mavlink.param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - if (write_ret == OK) { - mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM"); - result = MAV_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params to EEPROM"); - } else { - mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params, no EEPROM found"); - } - result = MAV_RESULT_FAILED; - } - - } else { - //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n"); - mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported STOR request"); - result = MAV_RESULT_UNSUPPORTED; - } - } - } - - /* send back command result */ - //mavlink_msg_command_ack_send(chan, cmd.command, result); - } break; + // case MAVLINK_MSG_ID_COMMAND_LONG: { + // mavlink_command_long_t cmd_mavlink; + // mavlink_msg_command_long_decode(msg, &cmd_mavlink); + + // uint8_t result = MAV_RESULT_UNSUPPORTED; + + // if (cmd_mavlink.target_system == mavlink_system.sysid && + // ((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + + // // XXX move this to LOW PRIO THREAD of commander app + + // /* preflight parameter load / store */ + // if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) { + // /* Read all parameters from EEPROM to RAM */ + + // if (((int)(cmd_mavlink.param1)) == 0) { + + // /* read all parameters from EEPROM to RAM */ + // int read_ret = param_load_default(); + // if (read_ret == OK) { + // //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); + // mavlink_missionlib_send_gcs_string("[pm] OK loading %s", param_get_default_file()); + // result = MAV_RESULT_ACCEPTED; + // } else if (read_ret == 1) { + // mavlink_missionlib_send_gcs_string("[pm] OK no changes %s", param_get_default_file()); + // result = MAV_RESULT_ACCEPTED; + // } else { + // if (read_ret < -1) { + // mavlink_missionlib_send_gcs_string("[pm] ERR loading %s", param_get_default_file()); + // } else { + // mavlink_missionlib_send_gcs_string("[pm] ERR no file %s", param_get_default_file()); + // } + // result = MAV_RESULT_FAILED; + // } + + // } else if (((int)(cmd_mavlink.param1)) == 1) { + + // /* write all parameters from RAM to EEPROM */ + // int write_ret = param_save_default(); + // if (write_ret == OK) { + // mavlink_missionlib_send_gcs_string("[pm] OK saved %s", param_get_default_file()); + // result = MAV_RESULT_ACCEPTED; + + // } else { + // if (write_ret < -1) { + // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file()); + // } else { + // mavlink_missionlib_send_gcs_string("[pm] ERR writing %s", param_get_default_file()); + // } + // result = MAV_RESULT_FAILED; + // } + + // } else { + // //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n"); + // mavlink_missionlib_send_gcs_string("[pm] refusing unsupp. STOR request"); + // result = MAV_RESULT_UNSUPPORTED; + // } + // } + // } + + // /* send back command result */ + // //mavlink_msg_command_ack_send(chan, cmd.command, result); + // } break; } } diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 826eb5625..39e574c04 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -253,12 +253,12 @@ handle_message(mavlink_message_t *msg) break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid] == 0) { + if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { ml_armed = false; } @@ -397,10 +397,6 @@ handle_message(mavlink_message_t *msg) rc_hil.timestamp = hrt_absolute_time(); rc_hil.chan_count = 4; - rc_hil.chan[0].raw = 1500 + man.x / 2; - rc_hil.chan[1].raw = 1500 + man.y / 2; - rc_hil.chan[2].raw = 1500 + man.r / 2; - rc_hil.chan[3].raw = 1500 + man.z / 2; rc_hil.chan[0].scaled = man.x / 1000.0f; rc_hil.chan[1].scaled = man.y / 1000.0f; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 08bf091cb..a6ae94495 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -67,6 +67,7 @@ struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; +struct rc_input_values rc_raw; struct actuator_armed_s armed; struct mavlink_subscriptions mavlink_subs; @@ -99,6 +100,7 @@ static void l_vehicle_attitude(struct listener *l); static void l_vehicle_gps_position(struct listener *l); static void l_vehicle_status(struct listener *l); static void l_rc_channels(struct listener *l); +static void l_input_rc(struct listener *l); static void l_global_position(struct listener *l); static void l_local_position(struct listener *l); static void l_global_position_setpoint(struct listener *l); @@ -116,6 +118,7 @@ struct listener listeners[] = { {l_vehicle_gps_position, &mavlink_subs.gps_sub, 0}, {l_vehicle_status, &status_sub, 0}, {l_rc_channels, &rc_sub, 0}, + {l_input_rc, &mavlink_subs.input_rc_sub, 0}, {l_global_position, &mavlink_subs.global_pos_sub, 0}, {l_local_position, &mavlink_subs.local_pos_sub, 0}, {l_global_position_setpoint, &mavlink_subs.spg_sub, 0}, @@ -274,21 +277,29 @@ l_rc_channels(struct listener *l) { /* copy rc channels into local buffer */ orb_copy(ORB_ID(rc_channels), rc_sub, &rc); + // XXX Add RC channels scaled message here +} + +void +l_input_rc(struct listener *l) +{ + /* copy rc channels into local buffer */ + orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw); if (gcs_link) /* Channels are sent in MAVLink main loop at a fixed interval */ mavlink_msg_rc_channels_raw_send(chan, - rc.timestamp / 1000, + rc_raw.timestamp / 1000, 0, - rc.chan[0].raw, - rc.chan[1].raw, - rc.chan[2].raw, - rc.chan[3].raw, - rc.chan[4].raw, - rc.chan[5].raw, - rc.chan[6].raw, - rc.chan[7].raw, - rc.rssi); + (rc_raw.channel_count > 0) ? rc_raw.values[0] : UINT16_MAX, + (rc_raw.channel_count > 1) ? rc_raw.values[1] : UINT16_MAX, + (rc_raw.channel_count > 2) ? rc_raw.values[2] : UINT16_MAX, + (rc_raw.channel_count > 3) ? rc_raw.values[3] : UINT16_MAX, + (rc_raw.channel_count > 4) ? rc_raw.values[4] : UINT16_MAX, + (rc_raw.channel_count > 5) ? rc_raw.values[5] : UINT16_MAX, + (rc_raw.channel_count > 6) ? rc_raw.values[6] : UINT16_MAX, + (rc_raw.channel_count > 7) ? rc_raw.values[7] : UINT16_MAX, + 255); } void @@ -583,6 +594,10 @@ uorb_receive_start(void) rc_sub = orb_subscribe(ORB_ID(rc_channels)); orb_set_interval(rc_sub, 100); /* 10Hz updates */ + /* --- RC RAW VALUE --- */ + mavlink_subs.input_rc_sub = orb_subscribe(ORB_ID(input_rc)); + orb_set_interval(mavlink_subs.input_rc_sub, 100); + /* --- GLOBAL POS VALUE --- */ mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ diff --git a/apps/mavlink/orb_topics.h b/apps/mavlink/orb_topics.h index f041e7c4c..6860702d2 100644 --- a/apps/mavlink/orb_topics.h +++ b/apps/mavlink/orb_topics.h @@ -57,6 +57,7 @@ #include <uORB/topics/actuator_controls.h> #include <uORB/topics/manual_control_setpoint.h> #include <uORB/topics/debug_key_value.h> +#include <drivers/drv_rc_input.h> struct mavlink_subscriptions { int sensor_sub; @@ -75,6 +76,7 @@ struct mavlink_subscriptions { int spl_sub; int spg_sub; int debug_key_value; + int input_rc_sub; }; extern struct mavlink_subscriptions mavlink_subs; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 63c296f96..29463d744 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -220,9 +220,9 @@ mc_thread_main(int argc, char *argv[]) /* only move setpoint if manual input is != 0 */ // XXX turn into param - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.25f) { + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { att_sp.yaw_body = att_sp.yaw_body + manual.yaw * 0.0025f; - } else if (manual.throttle <= 0.25f) { + } else if (manual.throttle <= 0.3f) { att_sp.yaw_body = att.yaw; } att_sp.thrust = manual.throttle; diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index 003180c18..839d56d29 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -57,50 +57,50 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); +//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); +//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); -PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); +//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); -PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); +//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); struct mc_att_control_params { float yaw_p; float yaw_i; float yaw_d; - float yaw_awu; - float yaw_lim; + //float yaw_awu; + //float yaw_lim; float att_p; float att_i; float att_d; - float att_awu; - float att_lim; + //float att_awu; + //float att_lim; - float att_xoff; - float att_yoff; + //float att_xoff; + //float att_yoff; }; struct mc_att_control_param_handles { param_t yaw_p; param_t yaw_i; param_t yaw_d; - param_t yaw_awu; - param_t yaw_lim; + //param_t yaw_awu; + //param_t yaw_lim; param_t att_p; param_t att_i; param_t att_d; - param_t att_awu; - param_t att_lim; + //param_t att_awu; + //param_t att_lim; - param_t att_xoff; - param_t att_yoff; + //param_t att_xoff; + //param_t att_yoff; }; /** @@ -122,17 +122,17 @@ static int parameters_init(struct mc_att_control_param_handles *h) h->yaw_p = param_find("MC_YAWPOS_P"); h->yaw_i = param_find("MC_YAWPOS_I"); h->yaw_d = param_find("MC_YAWPOS_D"); - h->yaw_awu = param_find("MC_YAWPOS_AWU"); - h->yaw_lim = param_find("MC_YAWPOS_LIM"); + //h->yaw_awu = param_find("MC_YAWPOS_AWU"); + //h->yaw_lim = param_find("MC_YAWPOS_LIM"); h->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); h->att_d = param_find("MC_ATT_D"); - h->att_awu = param_find("MC_ATT_AWU"); - h->att_lim = param_find("MC_ATT_LIM"); + //h->att_awu = param_find("MC_ATT_AWU"); + //h->att_lim = param_find("MC_ATT_LIM"); - h->att_xoff = param_find("MC_ATT_XOFF"); - h->att_yoff = param_find("MC_ATT_YOFF"); + //h->att_xoff = param_find("MC_ATT_XOFF"); + //h->att_yoff = param_find("MC_ATT_YOFF"); return OK; } @@ -142,17 +142,17 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc param_get(h->yaw_p, &(p->yaw_p)); param_get(h->yaw_i, &(p->yaw_i)); param_get(h->yaw_d, &(p->yaw_d)); - param_get(h->yaw_awu, &(p->yaw_awu)); - param_get(h->yaw_lim, &(p->yaw_lim)); + //param_get(h->yaw_awu, &(p->yaw_awu)); + //param_get(h->yaw_lim, &(p->yaw_lim)); param_get(h->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); param_get(h->att_d, &(p->att_d)); - param_get(h->att_awu, &(p->att_awu)); - param_get(h->att_lim, &(p->att_lim)); + //param_get(h->att_awu, &(p->att_awu)); + //param_get(h->att_lim, &(p->att_lim)); - param_get(h->att_xoff, &(p->att_xoff)); - param_get(h->att_yoff, &(p->att_yoff)); + //param_get(h->att_xoff, &(p->att_xoff)); + //param_get(h->att_yoff, &(p->att_yoff)); return OK; } @@ -186,10 +186,10 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - p.att_lim, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, - p.att_lim, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -202,18 +202,18 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* apply parameters */ printf("att ctrl: delays: %d us sens->ctrl, rate: %d Hz, input: %d Hz\n", sensor_delay, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu, p.att_lim); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } /* calculate current control outputs */ /* control pitch (forward) output */ - rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + p.att_xoff, + rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , att->pitch, att->pitchspeed, deltaT); /* control roll (left/right) output */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff, + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); /* control yaw rate */ diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c index a5bff97e6..d5e20312b 100644 --- a/apps/multirotor_att_control/multirotor_rate_control.c +++ b/apps/multirotor_att_control/multirotor_rate_control.c @@ -58,28 +58,28 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.1f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); -PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); +//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f); +//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f); PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */ PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.05f); PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); -PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ +//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f); +//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f); /**< roughly < 500 deg/s limit */ struct mc_rate_control_params { float yawrate_p; float yawrate_d; float yawrate_i; - float yawrate_awu; - float yawrate_lim; + //float yawrate_awu; + //float yawrate_lim; float attrate_p; float attrate_d; float attrate_i; - float attrate_awu; - float attrate_lim; + //float attrate_awu; + //float attrate_lim; float rate_lim; }; @@ -89,14 +89,14 @@ struct mc_rate_control_param_handles { param_t yawrate_p; param_t yawrate_i; param_t yawrate_d; - param_t yawrate_awu; - param_t yawrate_lim; + //param_t yawrate_awu; + //param_t yawrate_lim; param_t attrate_p; param_t attrate_i; param_t attrate_d; - param_t attrate_awu; - param_t attrate_lim; + //param_t attrate_awu; + //param_t attrate_lim; }; /** @@ -118,14 +118,14 @@ static int parameters_init(struct mc_rate_control_param_handles *h) h->yawrate_p = param_find("MC_YAWRATE_P"); h->yawrate_i = param_find("MC_YAWRATE_I"); h->yawrate_d = param_find("MC_YAWRATE_D"); - h->yawrate_awu = param_find("MC_YAWRATE_AWU"); - h->yawrate_lim = param_find("MC_YAWRATE_LIM"); + //h->yawrate_awu = param_find("MC_YAWRATE_AWU"); + //h->yawrate_lim = param_find("MC_YAWRATE_LIM"); h->attrate_p = param_find("MC_ATTRATE_P"); h->attrate_i = param_find("MC_ATTRATE_I"); h->attrate_d = param_find("MC_ATTRATE_D"); - h->attrate_awu = param_find("MC_ATTRATE_AWU"); - h->attrate_lim = param_find("MC_ATTRATE_LIM"); + //h->attrate_awu = param_find("MC_ATTRATE_AWU"); + //h->attrate_lim = param_find("MC_ATTRATE_LIM"); return OK; } @@ -135,14 +135,14 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru param_get(h->yawrate_p, &(p->yawrate_p)); param_get(h->yawrate_i, &(p->yawrate_i)); param_get(h->yawrate_d, &(p->yawrate_d)); - param_get(h->yawrate_awu, &(p->yawrate_awu)); - param_get(h->yawrate_lim, &(p->yawrate_lim)); + //param_get(h->yawrate_awu, &(p->yawrate_awu)); + //param_get(h->yawrate_lim, &(p->yawrate_lim)); param_get(h->attrate_p, &(p->attrate_p)); param_get(h->attrate_i, &(p->attrate_i)); param_get(h->attrate_d, &(p->attrate_d)); - param_get(h->attrate_awu, &(p->attrate_awu)); - param_get(h->attrate_lim, &(p->attrate_lim)); + //param_get(h->attrate_awu, &(p->attrate_awu)); + //param_get(h->attrate_lim, &(p->attrate_lim)); return OK; } diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c index 3730272a2..f21dd115b 100644 --- a/apps/px4/tests/test_hrt.c +++ b/apps/px4/tests/test_hrt.c @@ -121,7 +121,6 @@ int test_ppm(int argc, char *argv[]) int test_tone(int argc, char *argv[]) { -#ifdef CONFIG_TONE_ALARM int fd, result; unsigned long tone; @@ -171,7 +170,6 @@ out: if (fd >= 0) close(fd); -#endif return 0; } diff --git a/apps/px4io/Makefile b/apps/px4io/Makefile index 9b63d3ac8..801695f84 100644 --- a/apps/px4io/Makefile +++ b/apps/px4io/Makefile @@ -39,11 +39,10 @@ # Note that we pull a couple of specific files from the systemlib, since # we can't support it all. # -CSRCS = comms.c \ - mixer.c \ - px4io.c \ - safety.c \ - ../systemlib/hx_stream.c \ - ../systemlib/perf_counter.c +CSRCS = $(wildcard *.c) \ + ../systemlib/hx_stream.c \ + ../systemlib/perf_counter.c + +INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common include $(APPDIR)/mk/app.mk diff --git a/apps/px4io/comms.c b/apps/px4io/comms.c index f9106d830..1edff25b1 100644 --- a/apps/px4io/comms.c +++ b/apps/px4io/comms.c @@ -45,6 +45,7 @@ #include <stdlib.h> #include <errno.h> #include <string.h> +#include <poll.h> #include <nuttx/clock.h> @@ -81,7 +82,6 @@ comms_check(void) { static hrt_abstime last_report_time; hrt_abstime now, delta; - uint8_t c; /* should we send a report to the FMU? */ now = hrt_absolute_time(); @@ -93,7 +93,7 @@ comms_check(void) last_report_time = now; /* populate the report */ - for (unsigned i = 0; i < system_state.rc_channels; i++) + for (int i = 0; i < system_state.rc_channels; i++) report.rc_channel[i] = system_state.rc_channel_data[i]; report.channel_count = system_state.rc_channels; report.armed = system_state.armed; @@ -102,21 +102,47 @@ comms_check(void) hx_stream_send(stream, &report, sizeof(report)); } - /* feed any received bytes to the HDLC receive engine */ - while (read(fmu_fd, &c, 1) == 1) - hx_stream_rx(stream, c); + /* + * Check for bytes and feed them to the RX engine. + * Limit the number of bytes we actually process on any one iteration. + */ + char buf[32]; + ssize_t count = read(fmu_fd, buf, sizeof(buf)); + for (int i = 0; i < count; i++) + hx_stream_rx(stream, buf[i]); } +int frame_rx; +int frame_bad; + static void -comms_handle_frame(void *arg, const void *buffer, size_t length) +comms_handle_config(const void *buffer, size_t length) { - struct px4io_command *cmd; + const struct px4io_config *cfg = (struct px4io_config *)buffer; - /* make sure it's what we are expecting */ - if (length != sizeof(struct px4io_command)) + if (length != sizeof(*cfg)) { + frame_bad++; + return; + } + + frame_rx++; + + mixer_set_serial_mode(cfg->serial_rx_mode); + +} + +static void +comms_handle_command(const void *buffer, size_t length) +{ + const struct px4io_command *cmd = (struct px4io_command *)buffer; + + if (length != sizeof(*cmd)) { + frame_bad++; return; + } - cmd = (struct px4io_command *)buffer; + frame_rx++; + irqstate_t flags = irqsave(); /* fetch new PWM output values */ for (unsigned i = 0; i < PX4IO_OUTPUT_CHANNELS; i++) @@ -133,4 +159,30 @@ comms_handle_frame(void *arg, const void *buffer, size_t length) /* XXX do relay changes here */ for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) system_state.relays[i] = cmd->relay_state[i]; + + irqrestore(flags); } + + +static void +comms_handle_frame(void *arg, const void *buffer, size_t length) +{ + const uint16_t *type = (const uint16_t *)buffer; + + + /* make sure it's what we are expecting */ + if (length > 2) { + switch (*type) { + case F2I_MAGIC: + comms_handle_command(buffer, length); + break; + case F2I_CONFIG_MAGIC: + comms_handle_config(buffer, length); + break; + default: + break; + } + } + frame_bad++; +} + diff --git a/apps/px4io/mixer.c b/apps/px4io/mixer.c index a91aad3ca..471965fd7 100644 --- a/apps/px4io/mixer.c +++ b/apps/px4io/mixer.c @@ -40,22 +40,20 @@ #include <sys/types.h> #include <stdbool.h> - +#include <string.h> #include <assert.h> #include <errno.h> #include <fcntl.h> +#include <termios.h> +#include <unistd.h> +#include <fcntl.h> -#include <arch/board/drv_ppm_input.h> -#include <arch/board/drv_pwm_servo.h> +#include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> -#include "px4io.h" +#include <systemlib/ppm_decode.h> -#ifdef CONFIG_DISABLE_MQUEUE -# error Mixer requires message queues - set CONFIG_DISABLE_MQUEUE=n and try again -#endif - -static mqd_t input_queue; +#include "px4io.h" /* * Count of periodic calls in which we have no data. @@ -67,7 +65,12 @@ static unsigned mixer_input_drops; * Count of periodic calls in which we have no FMU input. */ static unsigned fmu_input_drops; -#define FMU_INPUT_DROP_LIMIT 10 +#define FMU_INPUT_DROP_LIMIT 20 + +/* + * Serial port fd for serial RX protocols + */ +static int rx_port = -1; /* * HRT periodic call used to check for control input data. @@ -89,9 +92,6 @@ static void mixer_get_rc_input(void); */ static void mixer_update(int mixer, uint16_t *inputs, int input_count); -/* servo driver handle */ -int mixer_servo_fd; - /* current servo arm/disarm state */ bool mixer_servos_armed; @@ -104,15 +104,10 @@ struct mixer { } mixers[IO_SERVO_COUNT]; int -mixer_init(const char *mq_name) +mixer_init(void) { - /* open the control input queue; this should always exist */ - input_queue = mq_open(mq_name, O_RDONLY | O_NONBLOCK); - ASSERTCODE((input_queue >= 0), A_INPUTQ_OPEN_FAIL); - - /* open the servo driver */ - mixer_servo_fd = open("/dev/pwm_servo", O_WRONLY); - ASSERTCODE((mixer_servo_fd >= 0), A_SERVO_OPEN_FAIL); + /* open the serial port */ + rx_port = open("/dev/ttyS0", O_RDONLY | O_NONBLOCK); /* look for control data at 50Hz */ hrt_call_every(&mixer_input_call, 1000, 20000, mixer_tick, NULL); @@ -120,6 +115,38 @@ mixer_init(const char *mq_name) return 0; } +void +mixer_set_serial_mode(uint8_t serial_mode) +{ + + if (serial_mode == system_state.serial_rx_mode) + return; + + struct termios t; + tcgetattr(rx_port, &t); + + switch (serial_mode) { + case RX_MODE_PPM_ONLY: + break; + case RX_MODE_SPEKTRUM_6: + case RX_MODE_SPEKTRUM_7: + /* 115200, no parity, one stop bit */ + cfsetspeed(&t, 115200); + t.c_cflag &= ~(CSTOPB | PARENB); + break; + case RX_MODE_FUTABA_SBUS: + /* 100000, even parity, two stop bits */ + cfsetspeed(&t, 100000); + t.c_cflag |= (CSTOPB | PARENB); + break; + default: + return; + } + + tcsetattr(rx_port, TCSANOW, &t); + system_state.serial_rx_mode = serial_mode; +} + static void mixer_tick(void *arg) { @@ -176,9 +203,8 @@ mixer_tick(void *arg) * If we are armed, update the servo output. */ if (system_state.armed) - ioctl(mixer_servo_fd, PWM_SERVO_SET(i), mixers[i].current_value); + up_pwm_servo_set(i, mixers[i].current_value); } - } /* @@ -187,34 +213,155 @@ mixer_tick(void *arg) should_arm = system_state.armed && (control_count > 0); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ - ioctl(mixer_servo_fd, PWM_SERVO_ARM, 0); + up_pwm_servo_arm(true); mixer_servos_armed = true; } else if (!should_arm && mixer_servos_armed) { - /* armed but need to disarm*/ - ioctl(mixer_servo_fd, PWM_SERVO_DISARM, 0); + /* armed but need to disarm */ + up_pwm_servo_arm(false); mixer_servos_armed = false; } } static void -mixer_get_rc_input(void) +mixer_update(int mixer, uint16_t *inputs, int input_count) +{ + /* simple passthrough for now */ + if (mixer < input_count) { + mixers[mixer].current_value = inputs[mixer]; + } else { + mixers[mixer].current_value = 0; + } +} + +static bool +mixer_get_spektrum_input(void) { - ssize_t len; + static uint8_t buf[16]; + static unsigned count; + + /* always read as much data as we can into the buffer */ + if (count >= sizeof(buf)) + count = 0; + ssize_t result = read(rx_port, buf, sizeof(buf) - count); + /* no data or an error */ + if (result <= 0) + return false; + count += result; + + /* if there are more than two bytes in the buffer, check for sync */ + if (count >= 2) { + if ((buf[0] != 0x3) || (buf[1] != 0x1)) { + /* not in sync; look for a possible sync marker */ + for (unsigned i = 1; i < count; i++) { + if (buf[i] == 0x3) { + /* could be a frame marker; move buffer bytes */ + count -= i; + memmove(buf, buf + i, count); + break; + } + } + } + } + if (count < sizeof(buf)) + return false; + + /* we got a frame; decode it */ + const uint16_t *channels = (const uint16_t *)&buf[2]; /* - * Pull channel data from the message queue into the system state structure. + * Channel assignment for DX6i vs. DX7 is different. + * + * DX7 etc. is: * + * 0: Aileron + * 1: Flaps + * 2: Gear + * 3: Elevator + * 4: Aux2 + * 5: Throttle + * 6: Rudder + * + * DX6i is: + * + * 0: Aileron + * 1: Flaps + * 2: Elevator + * 3: Rudder + * 4: Throttle + * 5: Gear + * 6: <notused> + * + * We convert these to our standard Futaba-style assignment: + * + * 0: Throttle (Throttle) + * 1: Roll (Aileron) + * 2: Pitch (Elevator) + * 3: Yaw (Rudder) + * 4: Override (Flaps) + * 5: FUNC_0 (Gear) + * 6: FUNC_1 (Aux2) */ - len = mq_receive(input_queue, &system_state.rc_channel_data, sizeof(system_state.rc_channel_data), NULL); + if (system_state.serial_rx_mode == RX_MODE_SPEKTRUM_7) { + system_state.rc_channel_data[0] = channels[5]; /* Throttle */ + system_state.rc_channel_data[1] = channels[0]; /* Roll */ + system_state.rc_channel_data[2] = channels[3]; /* Pitch */ + system_state.rc_channel_data[3] = channels[6]; /* Yaw */ + system_state.rc_channel_data[4] = channels[1]; /* Override */ + system_state.rc_channel_data[5] = channels[2]; /* FUNC_0 */ + system_state.rc_channel_data[6] = channels[4]; /* FUNC_1 */ + system_state.rc_channels = 7; + } else { + system_state.rc_channel_data[0] = channels[4]; /* Throttle */ + system_state.rc_channel_data[1] = channels[0]; /* Roll */ + system_state.rc_channel_data[2] = channels[2]; /* Pitch */ + system_state.rc_channel_data[3] = channels[3]; /* Yaw */ + system_state.rc_channel_data[4] = channels[1]; /* Override */ + system_state.rc_channel_data[5] = channels[5]; /* FUNC_0 */ + system_state.rc_channels = 6; + } + count = 0; + return true; +} - /* - * If we have data, update the count and status. - */ - if (len > 0) { - system_state.rc_channels = len / sizeof(system_state.rc_channel_data[0]); - mixer_input_drops = 0; +static bool +mixer_get_sbus_input(void) +{ + /* XXX not implemented yet */ + return false; +} + +static void +mixer_get_rc_input(void) +{ + bool got_input = false; + + switch (system_state.serial_rx_mode) { + case RX_MODE_PPM_ONLY: + if (ppm_decoded_channels > 0) { + /* copy channel data */ + system_state.rc_channels = ppm_decoded_channels; + for (unsigned i = 0; i < ppm_decoded_channels; i++) + system_state.rc_channel_data[i] = ppm_buffer[i]; + got_input = true; + } + break; + case RX_MODE_SPEKTRUM_6: + case RX_MODE_SPEKTRUM_7: + got_input = mixer_get_spektrum_input(); + break; + + case RX_MODE_FUTABA_SBUS: + got_input = mixer_get_sbus_input(); + break; + + default: + break; + } + + if (got_input) { + mixer_input_drops = 0; system_state.fmu_report_due = true; } else { /* @@ -232,14 +379,3 @@ mixer_get_rc_input(void) } } } - -static void -mixer_update(int mixer, uint16_t *inputs, int input_count) -{ - /* simple passthrough for now */ - if (mixer < input_count) { - mixers[mixer].current_value = inputs[mixer]; - } else { - mixers[mixer].current_value = 0; - } -} diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index edc2c4bd2..7467f1adc 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -48,7 +48,7 @@ #define PX4IO_OUTPUT_CHANNELS 8 #define PX4IO_INPUT_CHANNELS 12 -#define PX4IO_RELAY_CHANNELS 2 +#define PX4IO_RELAY_CHANNELS 4 #pragma pack(push, 1) @@ -62,6 +62,18 @@ struct px4io_command { bool arm_ok; }; +/* config message from FMU to IO */ +struct px4io_config { + uint16_t f2i_config_magic; +#define F2I_CONFIG_MAGIC 0x6366 + + uint8_t serial_rx_mode; +#define RX_MODE_PPM_ONLY 0 +#define RX_MODE_SPEKTRUM_6 1 +#define RX_MODE_SPEKTRUM_7 2 +#define RX_MODE_FUTABA_SBUS 3 +}; + /* report from IO to FMU */ struct px4io_report { uint16_t i2f_magic; diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index a67db9a7e..7039e5d58 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -47,9 +47,7 @@ #include <nuttx/clock.h> -#include <arch/board/up_boardinitialize.h> -#include <arch/board/drv_gpio.h> -#include <arch/board/drv_ppm_input.h> +#include <drivers/drv_pwm_output.h> #include <drivers/drv_hrt.h> #include "px4io.h" @@ -61,8 +59,6 @@ int gpio_fd; static const char cursor[] = {'|', '/', '-', '\\'}; -static const char *rc_input_mq_name = "rc_input"; - static struct hrt_call timer_tick_call; volatile int timers[TIMER_NUM_TIMERS]; static void timer_tick(void *arg); @@ -73,8 +69,15 @@ int user_start(int argc, char *argv[]) bool heartbeat = false; bool failsafe = false; - /* Do board init */ - (void)up_boardinitialize(); + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* init the FMU link */ + comms_init(); + + /* configure the first 8 PWM outputs (i.e. all of them) */ + /* note, must do this after comms init to steal back PA0, which is CTS otherwise */ + up_pwm_servo_init(0xff); /* print some startup info */ lib_lowprintf("\nPX4IO: starting\n"); @@ -84,32 +87,22 @@ int user_start(int argc, char *argv[]) /* start the software timer service */ hrt_call_every(&timer_tick_call, 1000, 1000, timer_tick, NULL); - /* Open the GPIO driver so we can do LEDs and the like. */ - gpio_fd = open("/dev/gpio", 0); - ASSERTCODE((gpio_fd >= 0), A_GPIO_OPEN_FAIL); - /* default all the LEDs to off while we start */ - LED_AMBER(heartbeat); - LED_BLUE(failsafe); + LED_AMBER(false); + LED_BLUE(false); LED_SAFETY(false); /* turn on servo power */ POWER_SERVO(true); - /* configure the PPM input driver */ - ppm_input_init(rc_input_mq_name); - /* start the mixer */ - mixer_init(rc_input_mq_name); + mixer_init(); /* start the safety switch handler */ safety_init(); - /* init the FMU link */ - comms_init(); - /* set up some timers for the main loop */ - timers[TIMER_BLINK_AMBER] = 250; /* heartbeat blink @ 2Hz */ + timers[TIMER_BLINK_BLUE] = 250; /* heartbeat blink @ 2Hz */ timers[TIMER_STATUS_PRINT] = 1000; /* print status message @ 1Hz */ /* @@ -121,33 +114,34 @@ int user_start(int argc, char *argv[]) comms_check(); /* blink the heartbeat LED */ - if (timers[TIMER_BLINK_AMBER] == 0) { - timers[TIMER_BLINK_AMBER] = 250; - LED_AMBER((heartbeat = !heartbeat)); + if (timers[TIMER_BLINK_BLUE] == 0) { + timers[TIMER_BLINK_BLUE] = 250; + LED_BLUE(heartbeat = !heartbeat); } /* blink the failsafe LED if we don't have FMU input */ if (!system_state.mixer_use_fmu) { - if (timers[TIMER_BLINK_BLUE] == 0) { - timers[TIMER_BLINK_BLUE] = 125; - LED_BLUE((failsafe = !failsafe)); + if (timers[TIMER_BLINK_AMBER] == 0) { + timers[TIMER_BLINK_AMBER] = 125; + failsafe = !failsafe; } } else { - LED_BLUE((failsafe = false)); + failsafe = false; } + LED_AMBER(failsafe); /* print some simple status */ if (timers[TIMER_STATUS_PRINT] == 0) { timers[TIMER_STATUS_PRINT] = 1000; - lib_lowprintf("%c %s | %s | %s | C=%d \r", + lib_lowprintf("%c %s | %s | %s | C=%d F=%d B=%d \r", cursor[cycle++ & 3], (system_state.armed ? "ARMED" : "SAFE"), (system_state.rc_channels ? "RC OK" : "NO RC"), (system_state.mixer_use_fmu ? "FMU OK" : "NO FMU"), - system_state.rc_channels + system_state.rc_channels, + frame_rx, frame_bad ); } - } /* Should never reach here */ diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index d90f7e36b..274b27ff3 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -32,10 +32,18 @@ ****************************************************************************/ /** - * @file General defines and structures for the PX4IO module firmware. + * @file px4io.h + * + * General defines and structures for the PX4IO module firmware. */ -#include <arch/board/drv_gpio.h> +#include <nuttx/config.h> + +#include <stdbool.h> +#include <stdint.h> + +#include <drivers/boards/px4io/px4io_internal.h> + #include "protocol.h" /* @@ -45,6 +53,16 @@ #define IO_SERVO_COUNT 8 /* + * Debug logging + */ + +#if 1 +# define debug(fmt, ...) lib_lowprintf(fmt "\n", ##args) +#else +# define debug(fmt, ...) do {} while(0) +#endif + +/* * System state structure. */ struct sys_state_s @@ -83,10 +101,19 @@ struct sys_state_s * If true, new control data from the FMU has been received. */ bool fmu_data_received; + + /* + * Current serial interface mode, per the serial_rx_mode parameter + * in the config packet. + */ + uint8_t serial_rx_mode; }; extern struct sys_state_s system_state; +extern int frame_rx; +extern int frame_bad; + /* * Software countdown timers. * @@ -102,26 +129,25 @@ extern volatile int timers[TIMER_NUM_TIMERS]; /* * GPIO handling. */ -extern int gpio_fd; - -#define POWER_SERVO(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_POWER), (_s)) -#define POWER_ACC1(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC1), (_s)) -#define POWER_ACC2(_s) ioctl(gpio_fd, GPIO_SET(GPIO_SERVO_ACC2), (_s)) -#define POWER_RELAY1(_s) ioctl(gpio_fd, GPIO_SET(GPIO_RELAY1, (_s)) -#define POWER_RELAY2(_s) ioctl(gpio_fd, GPIO_SET(GPIO_RELAY2, (_s)) +#define LED_BLUE(_s) stm32_gpiowrite(GPIO_LED1, !(_s)) +#define LED_AMBER(_s) stm32_gpiowrite(GPIO_LED2, !(_s)) +#define LED_SAFETY(_s) stm32_gpiowrite(GPIO_LED3, !(_s)) -#define LED_AMBER(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_AMBER), !(_s)) -#define LED_BLUE(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_BLUE), !(_s)) -#define LED_SAFETY(_s) ioctl(gpio_fd, GPIO_SET(GPIO_LED_SAFETY), !(_s)) +#define POWER_SERVO(_s) stm32_gpiowrite(GPIO_SERVO_PWR_EN, (_s)) +#define POWER_ACC1(_s) stm32_gpiowrite(GPIO_SERVO_ACC1_EN, (_s)) +#define POWER_ACC2(_s) stm32_gpiowrite(GPIO_SERVO_ACC2_EN, (_s)) +#define POWER_RELAY1(_s) stm32_gpiowrite(GPIO_RELAY1_EN, (_s)) +#define POWER_RELAY2(_s) stm32_gpiowrite(GPIO_RELAY2_EN, (_s)) -#define OVERCURRENT_ACC ioctl(gpio_fd, GPIO_GET(GPIO_ACC_OVERCURRENT), 0) -#define OVERCURRENT_SERVO ioctl(gpio_fd, GPIO_GET(GPIO_SERVO_OVERCURRENT), 0) -#define BUTTON_SAFETY ioctl(gpio_fd, GPIO_GET(GPIO_SAFETY_BUTTON), 0) +#define OVERCURRENT_ACC stm32_gpioread(GPIO_ACC_OC_DETECT) +#define OVERCURRENT_SERVO stm32_gpioread(GPIO_SERVO_OC_DETECT +#define BUTTON_SAFETY stm32_gpioread(GPIO_BTN_SAFETY) /* * Mixer */ -extern int mixer_init(const char *mq_name); +extern int mixer_init(void); +extern void mixer_set_serial_mode(uint8_t newmode); /* * Safety switch/LED. diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 6cb85798f..f27432664 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -46,9 +46,6 @@ #include <nuttx/clock.h> -#include <arch/board/up_boardinitialize.h> -#include <arch/board/drv_gpio.h> -#include <arch/board/drv_ppm_input.h> #include <drivers/drv_hrt.h> #include "px4io.h" diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 892ec975a..fd8a6602a 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -68,28 +68,28 @@ PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); PARAM_DEFINE_FLOAT(RC1_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC1_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC1_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC1_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC2_MIN, 1000); PARAM_DEFINE_FLOAT(RC2_TRIM, 1500); PARAM_DEFINE_FLOAT(RC2_MAX, 2000); PARAM_DEFINE_FLOAT(RC2_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC2_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC2_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC2_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC3_MIN, 1000); PARAM_DEFINE_FLOAT(RC3_TRIM, 1500); PARAM_DEFINE_FLOAT(RC3_MAX, 2000); PARAM_DEFINE_FLOAT(RC3_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC3_DZ, 10.0f); +PARAM_DEFINE_FLOAT(RC3_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC3_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC4_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); @@ -103,21 +103,21 @@ PARAM_DEFINE_FLOAT(RC6_MIN, 1000); PARAM_DEFINE_FLOAT(RC6_TRIM, 1500); PARAM_DEFINE_FLOAT(RC6_MAX, 2000); PARAM_DEFINE_FLOAT(RC6_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC6_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC6_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC6_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC7_MIN, 1000); PARAM_DEFINE_FLOAT(RC7_TRIM, 1500); PARAM_DEFINE_FLOAT(RC7_MAX, 2000); PARAM_DEFINE_FLOAT(RC7_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC7_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC7_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC7_EXP, 0.0f); PARAM_DEFINE_FLOAT(RC8_MIN, 1000); PARAM_DEFINE_FLOAT(RC8_TRIM, 1500); PARAM_DEFINE_FLOAT(RC8_MAX, 2000); PARAM_DEFINE_FLOAT(RC8_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC8_DZ, 0.1f); +PARAM_DEFINE_FLOAT(RC8_DZ, 0.0f); PARAM_DEFINE_FLOAT(RC8_EXP, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); // 1 = FUTABA diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index e4da687c6..3e9f35eaf 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -58,12 +58,15 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_mag.h> #include <drivers/drv_baro.h> +#include <drivers/drv_rc_input.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <systemlib/perf_counter.h> +#include <systemlib/ppm_decode.h> + #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/rc_channels.h> @@ -90,19 +93,6 @@ #define BAT_VOL_LOWPASS_2 0.01f #define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f -#ifdef CONFIG_HRT_PPM -extern "C" { - extern uint16_t ppm_buffer[]; - extern unsigned ppm_decoded_channels; - extern uint64_t ppm_last_valid_decode; -} - -/* PPM Settings */ -# define PPM_MIN 1000 -# define PPM_MAX 2000 -# define PPM_MID (PPM_MIN+PPM_MAX)/2 -#endif - /** * Sensor app start / stop handling function * @@ -155,6 +145,7 @@ private: int _gyro_sub; /**< raw gyro data subscription */ int _accel_sub; /**< raw accel data subscription */ int _mag_sub; /**< raw mag data subscription */ + int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ @@ -174,6 +165,7 @@ private: float rev[_rc_max_chan_count]; float dz[_rc_max_chan_count]; float ex[_rc_max_chan_count]; + float scaling_factor[_rc_max_chan_count]; float gyro_offset[3]; float mag_offset[3]; @@ -343,6 +335,7 @@ Sensors::Sensors() : _gyro_sub(-1), _accel_sub(-1), _mag_sub(-1), + _rc_sub(-1), _baro_sub(-1), _vstatus_sub(-1), _params_sub(-1), @@ -476,14 +469,13 @@ Sensors::parameters_update() warnx("Failed getting exponential gain for chan %d", i); } - _rc.chan[i].scaling_factor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); + _parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]); /* handle blowup in the scaling factor calculation */ - if (isnan(_rc.chan[i].scaling_factor) || isinf(_rc.chan[i].scaling_factor)) { - _rc.chan[i].scaling_factor = 0; + if (isnan(_parameters.scaling_factor[i]) || isinf(_parameters.scaling_factor[i])) { + _parameters.scaling_factor[i] = 0; } - _rc.chan[i].mid = _parameters.trim[i]; } /* update RC function mappings */ @@ -868,98 +860,125 @@ Sensors::adc_poll(struct sensor_combined_s &raw) void Sensors::ppm_poll() { - struct manual_control_setpoint_s manual_control; - - /* check to see whether a new frame has been decoded */ - if (_ppm_last_valid == ppm_last_valid_decode) - return; - /* require at least two chanels to consider the signal valid */ - if (ppm_decoded_channels < 4) - return; - - unsigned channel_limit = ppm_decoded_channels; - if (channel_limit > _rc_max_chan_count) - channel_limit = _rc_max_chan_count; - - /* we are accepting this decode */ - _ppm_last_valid = ppm_last_valid_decode; - - /* Read out values from HRT */ - for (unsigned int i = 0; i < channel_limit; i++) { - _rc.chan[i].raw = ppm_buffer[i]; - - /* scale around the mid point differently for lower and upper range */ - if (ppm_buffer[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (ppm_buffer[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); - } else if (ppm_buffer[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] - ppm_buffer[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); - - } else { - /* in the configured dead zone, output zero */ - _rc.chan[i].scaled = 0.0f; + /* 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; + + if (ppm_decoded_channels > 1) { + + for (int i = 0; i < ppm_decoded_channels; i++) { + raw.values[i] = ppm_buffer[i]; } - /* reverse channel if required */ - if (i == _rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } + 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 { - _rc.chan[i].scaled *= _parameters.rev[i]; + orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); } + } - /* handle any parameter-induced blowups */ - if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) - _rc.chan[i].scaled = 0.0f; - //_rc.chan[i].scaled = (ppm_buffer[i] - _rc.chan[i].mid) * _rc.chan[i].scaling_factor; - } + /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ + bool rc_updated; + orb_check(_rc_sub, &rc_updated); - _rc.chan_count = ppm_decoded_channels; - _rc.timestamp = ppm_last_valid_decode; + if (rc_updated) { + struct rc_input_values rc_input; - manual_control.timestamp = ppm_last_valid_decode; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; - if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; - if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; - if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { - manual_control.roll *= _parameters.rc_scale_roll; - } + struct manual_control_setpoint_s manual_control; - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; - if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; - if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; - if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + /* require at least two chanels to consider the signal valid */ + if (rc_input.channel_count < 2) + return; - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; - if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; - if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; - if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } - - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + unsigned channel_limit = rc_input.channel_count; + if (channel_limit > _rc_max_chan_count) + channel_limit = _rc_max_chan_count; + + /* we are accepting this message */ + _ppm_last_valid = rc_input.timestamp; + + /* 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 */ + 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]); + } 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])); + + } else { + /* in the configured dead zone, output zero */ + _rc.chan[i].scaled = 0.0f; + } - /* mode switch input */ - manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; - if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; - if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + /* reverse channel if required */ + if (i == _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]; + } + + /* handle any parameter-induced blowups */ + if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + _rc.chan[i].scaled = 0.0f; + } - orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); - orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + _rc.chan_count = rc_input.channel_count; + _rc.timestamp = rc_input.timestamp; + + manual_control.timestamp = rc_input.timestamp; + + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = _rc.chan[_rc.function[ROLL]].scaled; + if (manual_control.roll < -1.0f) manual_control.roll = -1.0f; + if (manual_control.roll > 1.0f) manual_control.roll = 1.0f; + if (!isnan(_parameters.rc_scale_roll) || !isinf(_parameters.rc_scale_roll)) { + manual_control.roll *= _parameters.rc_scale_roll; + } + + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = -1.0f * _rc.chan[_rc.function[PITCH]].scaled; + if (manual_control.pitch < -1.0f) manual_control.pitch = -1.0f; + if (manual_control.pitch > 1.0f) manual_control.pitch = 1.0f; + if (!isnan(_parameters.rc_scale_pitch) || !isinf(_parameters.rc_scale_pitch)) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } + + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = _rc.chan[_rc.function[YAW]].scaled * _parameters.rc_scale_yaw; + if (manual_control.yaw < -1.0f) manual_control.yaw = -1.0f; + if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; + if (!isnan(_parameters.rc_scale_yaw) || !isinf(_parameters.rc_scale_yaw)) { + manual_control.yaw *= _parameters.rc_scale_yaw; + } + + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + + /* mode switch input */ + manual_control.override_mode_switch = _rc.chan[_rc.function[OVERRIDE]].scaled; + if (manual_control.override_mode_switch < -1.0f) manual_control.override_mode_switch = -1.0f; + if (manual_control.override_mode_switch > 1.0f) manual_control.override_mode_switch = 1.0f; + + orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); + orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); + } } #endif @@ -991,6 +1010,7 @@ Sensors::task_main() _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); + _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); diff --git a/apps/systemcmds/param/param.c b/apps/systemcmds/param/param.c index 92313e45a..210841604 100644 --- a/apps/systemcmds/param/param.c +++ b/apps/systemcmds/param/param.c @@ -59,7 +59,7 @@ __EXPORT int param_main(int argc, char *argv[]); static void do_save(const char* param_file_name); static void do_load(const char* param_file_name); static void do_import(const char* param_file_name); -static void do_show(void); +static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); int @@ -96,13 +96,16 @@ param_main(int argc, char *argv[]) } else { param_set_default_file(NULL); } - warnx("selected parameter file %s", param_get_default_file()); + warnx("selected parameter default file %s", param_get_default_file()); + exit(0); } - if (!strcmp(argv[1], "show")) { - do_show(); - } - + if (!strcmp(argv[1], "show")) + if (argc >= 3) { + do_show(argv[2]); + } else { + do_show(NULL); + } } errx(1, "expected a command, try 'load', 'import', 'show', 'select' or 'save'"); @@ -144,9 +147,6 @@ do_load(const char* param_file_name) if (result < 0) { errx(1, "error importing from '%s'", param_file_name); - } else { - /* set default file name for next storage operation */ - param_set_default_file(param_file_name); } exit(0); @@ -170,10 +170,10 @@ do_import(const char* param_file_name) } static void -do_show(void) +do_show(const char* search_string) { printf(" + = saved, * = unsaved\n"); - param_foreach(do_show_print, NULL, false); + param_foreach(do_show_print, search_string, false); exit(0); } @@ -183,6 +183,11 @@ do_show_print(void *arg, param_t param) { int32_t i; float f; + const char *search_string = (const char*)arg; + + /* print nothing if search string valid and not matching */ + if (arg != NULL && (strcmp(search_string, param_name(param) != 0))) + return; printf("%c %s: ", param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), diff --git a/apps/systemlib/hx_stream.c b/apps/systemlib/hx_stream.c index 7ee9888ea..8d77f14a8 100644 --- a/apps/systemlib/hx_stream.c +++ b/apps/systemlib/hx_stream.c @@ -160,6 +160,11 @@ hx_stream_init(int fd, void hx_stream_free(hx_stream_t stream) { + /* free perf counters (OK if they are NULL) */ + perf_free(stream->pc_tx_frames); + perf_free(stream->pc_rx_frames); + perf_free(stream->pc_rx_errors); + free(stream); } @@ -186,10 +191,8 @@ hx_stream_send(hx_stream_t stream, const uint8_t *p = (const uint8_t *)data; unsigned resid = count; - if (resid > HX_STREAM_MAX_FRAME) { - errno = EINVAL; - return -1; - } + if (resid > HX_STREAM_MAX_FRAME) + return -EINVAL; /* start the frame */ hx_tx_raw(stream, FBO); @@ -214,10 +217,11 @@ hx_stream_send(hx_stream_t stream, /* check for transmit error */ if (stream->txerror) { stream->txerror = false; - return -1; + return -EIO; } - return -1; + perf_count(stream->pc_tx_frames); + return 0; } void diff --git a/apps/systemlib/hx_stream.h b/apps/systemlib/hx_stream.h index ac3b3e99d..128689953 100644 --- a/apps/systemlib/hx_stream.h +++ b/apps/systemlib/hx_stream.h @@ -102,8 +102,7 @@ __EXPORT extern void hx_stream_set_counters(hx_stream_t stream, * @param stream A handle returned from hx_stream_init. * @param data Pointer to the data to send. * @param count The number of bytes to send. - * @return Zero on success, nonzero with errno - * set on error. + * @return Zero on success, -errno on error. */ __EXPORT extern int hx_stream_send(hx_stream_t stream, const void *data, diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 9a00c91a5..ddf9b0975 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -482,7 +482,7 @@ param_reset_all(void) } static const char *param_default_file = "/eeprom/parameters"; -static char *param_user_file; +static char *param_user_file = NULL; int param_set_default_file(const char* filename) diff --git a/apps/systemlib/pid/pid.c b/apps/systemlib/pid/pid.c index 0358caa25..dd8b6adc6 100644 --- a/apps/systemlib/pid/pid.c +++ b/apps/systemlib/pid/pid.c @@ -172,9 +172,9 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the output. Limit output magnitude to pid->limit float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - if (output > pid->limit) output = pid->limit; + //if (output > pid->limit) output = pid->limit; - if (output < -pid->limit) output = -pid->limit; + //if (output < -pid->limit) output = -pid->limit; if (isfinite(output)) { pid->last_output = output; diff --git a/apps/systemlib/ppm_decode.h b/apps/systemlib/ppm_decode.h index c2b24321a..6c5e15345 100644 --- a/apps/systemlib/ppm_decode.h +++ b/apps/systemlib/ppm_decode.h @@ -46,12 +46,17 @@ */ #define PPM_MAX_CHANNELS 12 +/* PPM input nominal min/max values */ +#define PPM_MIN 1000 +#define PPM_MAX 2000 +#define PPM_MID ((PPM_MIN + PPM_MAX) / 2) + __BEGIN_DECLS /* * PPM decoder state */ -__EXPORT extern uint16_t ppm_buffer[]; /**< decoded PPM channel values */ +__EXPORT extern uint16_t ppm_buffer[PPM_MAX_CHANNELS]; /**< decoded PPM channel values */ __EXPORT extern unsigned ppm_decoded_channels; /**< count of decoded channels */ __EXPORT extern hrt_abstime ppm_last_valid_decode; /**< timestamp of the last valid decode */ diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index 2c487cf3b..fef6ef2b3 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -50,14 +50,6 @@ * @{ */ -enum RC_CHANNELS_STATUS -{ - UNKNOWN = 0, - KNOWN = 1, - SIGNAL = 2, - TIMEOUT = 3 -}; - /** * This defines the mapping of the RC functions. * The value assigned to the specific function corresponds to the entry of @@ -85,12 +77,7 @@ struct rc_channels_s { uint64_t timestamp; /**< In microseconds since boot time. */ uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */ struct { - uint16_t mid; /**< midpoint (0). */ - float scaling_factor; /**< scaling factor from raw counts to -1..+1 */ - uint16_t raw; /**< current raw value */ float scaled; /**< Scaled to -1..1 (throttle: 0..1) */ - uint16_t override; - enum RC_CHANNELS_STATUS status; /**< status of the channel */ } chan[RC_CHANNELS_FUNCTION_MAX]; uint8_t chan_count; /**< maximum number of valid channels */ @@ -98,6 +85,7 @@ struct rc_channels_s { char function_name[RC_CHANNELS_FUNCTION_MAX][20]; uint8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ + bool is_valid; /**< Inputs are valid, no timeout */ }; /**< radio control channels. */ /** diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig index 91cd0366d..9d61cae5b 100755 --- a/nuttx/configs/px4fmu/nsh/defconfig +++ b/nuttx/configs/px4fmu/nsh/defconfig @@ -306,25 +306,9 @@ CONFIG_USART6_RXDMA=y # set HRT_PPM_CHANNEL to the timer capture/compare channel to be # used, and define GPIO_PPM_IN to configure the appropriate timer # GPIO. -# CONFIG_TONE_ALARM -# Enables the tone alarm (buzzer) driver The board definition must -# set TONE_ALARM_TIMER and TONE_ALARM_CHANNEL to the timer and -# capture/compare channels to be used. -# CONFIG_PWM_SERVO -# Enables the PWM servo driver. The driver configuration must be -# supplied by the board support at initialisation time. -# Note that USART2 must be disabled on the PX4 board for this to -# be available. -# CONFIG_MULTIPORT -# Enabled support for run-time (or EEPROM based boot-time) configuration -# of ports for different functions (e.g. USART2 or ARDrone or PWM out) -# # CONFIG_HRT_TIMER=y CONFIG_HRT_PPM=y -CONFIG_TONE_ALARM=y -CONFIG_PWM_SERVO=n -CONFIG_MULTIPORT=n # # STM32F40xxx specific SPI device driver settings diff --git a/nuttx/configs/px4io/include/board.h b/nuttx/configs/px4io/include/board.h index cd4d48649..e9181baf1 100755 --- a/nuttx/configs/px4io/include/board.h +++ b/nuttx/configs/px4io/include/board.h @@ -47,9 +47,9 @@ # include <stdint.h> # include <stdbool.h> #endif -#include "stm32_rcc.h" -#include "stm32_sdio.h" -#include "stm32_internal.h" +#include <stm32_rcc.h> +#include <stm32_sdio.h> +#include <stm32_internal.h> /************************************************************************************ * Definitions @@ -116,12 +116,6 @@ # define GPIO_PPM_IN GPIO_TIM1_CH1IN #endif -/* - * PWM - * - * PWM configuration is provided via the configuration structure in up_boardinitialize.c - */ - /************************************************************************************ * Public Data ************************************************************************************/ @@ -151,17 +145,5 @@ extern "C" { EXTERN void stm32_boardinitialize(void); -/************************************************************************************ - * Power switch support. - */ -extern void up_power_init(void); -extern void up_power_set(int port, bool state); -extern bool up_power_error(int port); - -#undef EXTERN -#if defined(__cplusplus) -} -#endif - #endif /* __ASSEMBLY__ */ #endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx/configs/px4io/include/drv_gpio.h b/nuttx/configs/px4io/include/drv_gpio.h deleted file mode 100644 index 329d2bacf..000000000 --- a/nuttx/configs/px4io/include/drv_gpio.h +++ /dev/null @@ -1,67 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 GPIO driver for PX4IO - */ - -#include <sys/ioctl.h> - -#define _GPIO_IOCTL_BASE 0x7700 - -#define GPIO_SET(_x) _IOC(_GPIO_IOCTL_BASE, _x) -#define GPIO_GET(_x) _IOC(_GPIO_IOCTL_BASE + 1, _x) - -/* - * List of GPIOs; must be sorted with settable GPIOs first. - */ -#define GPIO_ACC1_POWER 0 /* settable */ -#define GPIO_ACC2_POWER 1 -#define GPIO_SERVO_POWER 2 -#define GPIO_RELAY1 3 -#define GPIO_RELAY2 4 -#define GPIO_LED_BLUE 5 -#define GPIO_LED_AMBER 6 -#define GPIO_LED_SAFETY 7 - -#define GPIO_ACC_OVERCURRENT 8 /* readonly */ -#define GPIO_SERVO_OVERCURRENT 9 -#define GPIO_SAFETY_BUTTON 10 - -#define GPIO_MAX_SETTABLE 7 -#define GPIO_MAX 10 - -/* - * GPIO driver init function. - */ -extern int gpio_drv_init(void); diff --git a/nuttx/configs/px4io/include/drv_ppm_input.h b/nuttx/configs/px4io/include/drv_ppm_input.h deleted file mode 100644 index 78c424154..000000000 --- a/nuttx/configs/px4io/include/drv_ppm_input.h +++ /dev/null @@ -1,100 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 PPM input decoder. - * - * Works in conjunction with the HRT driver, exports a device node - * and a message queue (if message queues are enabled). - * - * Note that the device node supports both blocking and non-blocking - * opens, but actually never blocks. A nonblocking open will return - * EWOULDBLOCK if there has not been an update since the last read, - * while a blocking open will always return the most recent data. - */ - -#include <sys/ioctl.h> - -#define _PPM_INPUT_BASE 0x7600 - -/* - * Fetch the state of the PPM input detector. - */ -#define PPM_INPUT_STATUS _IOC(_PPM_INPUT_BASE, 0) - -typedef enum { - PPM_STATUS_NO_SIGNAL = 0, - PPM_STATUS_SIGNAL_CURRENT = 1, -} ppm_input_status_t; - -/* - * Fetch the number of channels decoded (only valid when PPM_STATUS_SIGNAL_CURRENT). - */ -#define PPM_INPUT_CHANNELS _IOC(_PPM_INPUT_BASE, 1) - -typedef int ppm_input_channel_count_t; - -/* - * Device node - */ -#define PPM_DEVICE_NODE "/dev/ppm_input" - -/* - * Message queue; if message queues are supported, PPM input data is - * supplied to the queue when a frame is decoded. - */ -#ifndef CONFIG_DISABLE_MQUEUE -# define PPM_MESSAGE_QUEUE "ppm_input" -#endif - -/* - * Private hook from the HRT driver to the PPM decoder. - * - * This function is called for every edge of the incoming PPM - * signal. - * - * @param reset If true, the decoder should be reset (e.g.) - * capture failure was detected. - * @param count The counter value at which the edge - * was captured. - */ - -void ppm_input_decode(bool reset, uint16_t count); - -/* - * PPM input initialisation function. - * - * If message queues are enabled, and mq_name is not NULL, received input - * is posted to the message queue as an array of 16-bit unsigned channel values. - */ -int ppm_input_init(const char *mq_name);
\ No newline at end of file diff --git a/nuttx/configs/px4io/include/drv_pwm_servo.h b/nuttx/configs/px4io/include/drv_pwm_servo.h deleted file mode 100644 index 663468404..000000000 --- a/nuttx/configs/px4io/include/drv_pwm_servo.h +++ /dev/null @@ -1,94 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 PWM servo driver. - * - * The pwm_servo driver supports servos connected to STM32 timer - * blocks. - * - * Servo values can be set either with the PWM_SERVO_SET ioctl, or - * by writing an array of servo_position_t values to the device. - * Writing a value of 0 to a channel suppresses any output for that - * channel. - * - * Servo values can be read back either with the PWM_SERVO_GET - * ioctl, or by reading an array of servo_position_t values - * from the device. - * - * Attempts to set a channel that is not configured are ignored, - * and unconfigured channels always read zero. - * - * The PWM_SERVO_ARM / PWM_SERVO_DISARM calls globally arm - * (enable) and disarm (disable) all servo outputs. - */ - -#include <sys/ioctl.h> - -#define _PWM_SERVO_BASE 0x7500 -#define PWM_SERVO_ARM _IOC(_PWM_SERVO_BASE, 0) -#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1) - -#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) -#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo) - -typedef uint16_t servo_position_t; - -/* configuration limits */ -#define PWM_SERVO_MAX_TIMERS 3 -#define PWM_SERVO_MAX_CHANNELS 8 - -struct pwm_servo_config { - /* rate (in Hz) of PWM updates */ - uint32_t update_rate; - - /* array of timers dedicated to PWM servo use */ - struct pwm_servo_timer { - uint32_t base; - uint32_t clock_register; - uint32_t clock_bit; - uint32_t clock_freq; - } timers[PWM_SERVO_MAX_TIMERS]; - - /* array of channels in logical order */ - struct pwm_servo_channel { - uint32_t gpio; - uint8_t timer_index; - uint8_t timer_channel; - servo_position_t default_value; - } channels[PWM_SERVO_MAX_CHANNELS]; -}; - -extern int pwm_servo_init(const struct pwm_servo_config *config); - - diff --git a/nuttx/configs/px4io/include/up_boardinitialize.h b/nuttx/configs/px4io/include/up_boardinitialize.h deleted file mode 100755 index 01b9ca214..000000000 --- a/nuttx/configs/px4io/include/up_boardinitialize.h +++ /dev/null @@ -1,43 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Board initialisation prototype(s) - */ - -#ifndef __UP_BOARDINITIALIZE_H -#define __UP_BOARDINITIALIZE_H - -extern int up_boardinitialize(void); - -#endif diff --git a/nuttx/configs/px4io/include/up_hrt.h b/nuttx/configs/px4io/include/up_hrt.h deleted file mode 100644 index c83f14ac3..000000000 --- a/nuttx/configs/px4io/include/up_hrt.h +++ /dev/null @@ -1,129 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 High-resolution timer callouts and timekeeping. - */ - -#ifndef UP_HRT_H_ -#define UP_HRT_H_ - -#include <sys/types.h> -#include <stdbool.h> - -#include <time.h> -#include <queue.h> - -/* - * Absolute time, in microsecond units. - * - * Absolute time is measured from some arbitrary epoch shortly after - * system startup. It should never wrap or go backwards. - */ -typedef uint64_t hrt_abstime; - -/* - * Callout function type. - * - * Note that callouts run in the timer interrupt context, so - * they are serialised with respect to each other, and must not - * block. - */ -typedef void (* hrt_callout)(void *arg); - -/* - * Callout record. - */ -struct hrt_call { - struct sq_entry_s link; - - hrt_abstime deadline; - hrt_abstime period; - hrt_callout callout; - void *arg; -}; - -/* - * Get absolute time. - */ -extern hrt_abstime hrt_absolute_time(void); - -/* - * Convert a timespec to absolute time. - */ -extern hrt_abstime ts_to_abstime(struct timespec *ts); - -/* - * Convert absolute time to a timespec. - */ -extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); - -/* - * Call callout(arg) after delay has elapsed. - * - * If callout is NULL, this can be used to implement a timeout by testing the call - * with hrt_called(). - */ -extern void hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg); - -/* - * Call callout(arg) at absolute time calltime. - */ -extern void hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg); - -/* - * Call callout(arg) after delay, and then after every interval. - * - * Note thet the interval is timed between scheduled, not actual, call times, so the call rate may - * jitter but should not drift. - */ -extern void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg); - -/* - * If this returns true, the entry has been invoked and removed from the callout list. - * - * Always returns false for repeating callouts. - */ -extern bool hrt_called(struct hrt_call *entry); - -/* - * Remove the entry from the callout list. - */ -extern void hrt_cancel(struct hrt_call *entry); - -/* - * Initialise the HRT. - */ -extern void hrt_init(int timer); - -#endif /* UP_HRT_H_ */ diff --git a/nuttx/configs/px4io/io/appconfig b/nuttx/configs/px4io/io/appconfig index fbe8307a7..21a6fbacf 100644 --- a/nuttx/configs/px4io/io/appconfig +++ b/nuttx/configs/px4io/io/appconfig @@ -31,4 +31,7 @@ # ############################################################################ +CONFIGURED_APPS += drivers/boards/px4io +CONFIGURED_APPS += drivers/stm32 + CONFIGURED_APPS += px4io diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index eea430a46..e90e7ce62 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -84,7 +84,7 @@ CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n CONFIG_ARCH_LEDS=n -CONFIG_ARCH_BUTTONS=y +CONFIG_ARCH_BUTTONS=n CONFIG_ARCH_CALIBRATION=n CONFIG_ARCH_DMA=n CONFIG_ARMV7M_CMNVECTOR=y @@ -155,6 +155,8 @@ CONFIG_STM32_ADC3=n # CONFIG_USARTn_PARTIY - 0=no parity, 1=odd parity, 2=even parity # CONFIG_USARTn_2STOP - Two stop bits # +CONFIG_SERIAL_TERMIOS=y + CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n @@ -163,11 +165,11 @@ CONFIG_USART1_TXBUFSIZE=32 CONFIG_USART2_TXBUFSIZE=32 CONFIG_USART3_TXBUFSIZE=32 -CONFIG_USART1_RXBUFSIZE=32 -CONFIG_USART2_RXBUFSIZE=32 +CONFIG_USART1_RXBUFSIZE=64 +CONFIG_USART2_RXBUFSIZE=128 CONFIG_USART3_RXBUFSIZE=32 -CONFIG_USART1_BAUD=57600 +CONFIG_USART1_BAUD=115200 CONFIG_USART2_BAUD=115200 CONFIG_USART3_BAUD=115200 @@ -204,7 +206,6 @@ CONFIG_USART3_2STOP=0 # CONFIG_HRT_TIMER=y CONFIG_HRT_PPM=y -CONFIG_PWM_SERVO=y # # General build options @@ -389,10 +390,10 @@ CONFIG_DISABLE_CLOCK=n CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y CONFIG_DISABLE_SIGNALS=y -CONFIG_DISABLE_MQUEUE=n +CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=y +CONFIG_DISABLE_POLL=n # # Misc libc settings diff --git a/nuttx/configs/px4io/src/Makefile b/nuttx/configs/px4io/src/Makefile index 0ce004658..144fa8549 100644 --- a/nuttx/configs/px4io/src/Makefile +++ b/nuttx/configs/px4io/src/Makefile @@ -40,17 +40,7 @@ CFLAGS += -I$(TOPDIR)/sched ASRCS = AOBJS = $(ASRCS:.S=$(OBJEXT)) -CSRCS = up_boot.c up_hrt.c\ - drv_pwm_servo.c drv_ppm_input.c drv_gpio.c \ - up_boardinitialize.c - -ifeq ($(CONFIG_NSH_ARCHINIT),y) -CSRCS += up_nsh.c -endif - -ifeq ($(CONFIG_ADC),y) -CSRCS += up_adc.c -endif +CSRCS = empty.c COBJS = $(CSRCS:.c=$(OBJEXT)) diff --git a/nuttx/configs/px4io/src/drv_gpio.c b/nuttx/configs/px4io/src/drv_gpio.c deleted file mode 100644 index e53660a3c..000000000 --- a/nuttx/configs/px4io/src/drv_gpio.c +++ /dev/null @@ -1,110 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 GPIO driver for PX4IO. - */ - -#include <nuttx/config.h> - -#include <sys/types.h> -#include <stdbool.h> - -#include <errno.h> - -#include <arch/board/board.h> -#include <arch/board/drv_gpio.h> - -#include "px4io_internal.h" -#include "stm32_gpio.h" - -static int gpio_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations gpio_fops = { - .ioctl = gpio_ioctl -}; - -/* - * Order of initialisers in this array must match the order of - * GPIO_ definitions in drv_gpio.h - */ -static const uint32_t gpios[] = { - /* settable */ - GPIO_ACC1_PWR_EN, - GPIO_ACC2_PWR_EN, - GPIO_SERVO_PWR_EN, - GPIO_RELAY1_EN, - GPIO_RELAY2_EN, - GPIO_LED1, - GPIO_LED2, - GPIO_LED3, - - /* readonly */ - GPIO_ACC_OC_DETECT, - GPIO_SERVO_OC_DETECT, - GPIO_BTN_SAFETY -}; - -int -gpio_drv_init(void) -{ - int i; - - /* initialise GPIOs */ - for (i = 0; i < GPIO_MAX; i++) - if (gpios[i]) - stm32_configgpio(gpios[i]); - - /* register the device */ - return register_driver("/dev/gpio", &gpio_fops, 0666, NULL); -} - -static int -gpio_ioctl(struct file *filep, int cmd, unsigned long arg) -{ - /* attempt to set a GPIO? */ - if ((cmd >= GPIO_SET(0)) && (cmd <= GPIO_SET(GPIO_MAX_SETTABLE))) { - uint32_t gpio = gpios[cmd - GPIO_SET(0)]; - - if (gpio != 0) { - stm32_gpiowrite(gpio, arg ? true : false); - return 0; - } - } else if ((cmd >= GPIO_GET(0)) && (cmd <= GPIO_GET(GPIO_MAX))) { - uint32_t gpio = gpios[cmd - GPIO_GET(0)]; - - if (gpio != 0) - return stm32_gpioread(gpio) ? 1 : 0; - } - return -ENOTTY; -}
\ No newline at end of file diff --git a/nuttx/configs/px4io/src/drv_ppm_input.c b/nuttx/configs/px4io/src/drv_ppm_input.c deleted file mode 100644 index 2db0fbf0b..000000000 --- a/nuttx/configs/px4io/src/drv_ppm_input.c +++ /dev/null @@ -1,373 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 PPM input decoder. - * - * Works in conjunction with the HRT driver. - */ - - -#include <nuttx/config.h> -#include <nuttx/arch.h> -#include <nuttx/irq.h> - -#include <sys/types.h> -#include <stdbool.h> - -#include <assert.h> -#include <debug.h> -#include <time.h> -#include <queue.h> -#include <errno.h> -#include <string.h> -#include <stdio.h> -#include <fcntl.h> - -#include <arch/board/board.h> -#include <arch/board/drv_ppm_input.h> -#include <arch/board/up_hrt.h> - -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" - -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" - -#ifdef CONFIG_HRT_PPM -# ifndef CONFIG_HRT_TIMER -# error CONFIG_HRT_PPM requires CONFIG_HRT_TIMER -# endif - -/* - * PPM decoder tuning parameters. - * - * The PPM decoder works as follows. - * - * Initially, the decoder waits in the UNSYNCH state for two edges - * separated by PPM_MIN_START. Once the second edge is detected, - * the decoder moves to the ARM state. - * - * The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the - * timing mark for the first channel. If this is detected, it moves to - * the INACTIVE state. - * - * The INACTIVE phase waits for and discards the next edge, as it is not - * significant. Once the edge is detected, it moves to the ACTIVE stae. - * - * The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when - * received calculates the time from the previous mark and records - * this time as the value for the next channel. - * - * If at any time waiting for an edge, the delay from the previous edge - * exceeds PPM_MIN_START the frame is deemed to have ended and the recorded - * values are advertised to clients. - */ -#define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */ -#define PPM_MIN_CHANNEL_VALUE 800 /* shortest valid channel signal */ -#define PPM_MAX_CHANNEL_VALUE 2200 /* longest valid channel signal */ -#define PPM_MIN_START 2500 /* shortest valid start gap */ - -/* Input timeout - after this interval we assume signal is lost */ -#define PPM_INPUT_TIMEOUT 100 * 1000 /* 100ms */ - -/* Number of same-sized frames required to 'lock' */ -#define PPM_CHANNEL_LOCK 3 /* should be less than the input timeout */ - -/* decoded PPM buffer */ -#define PPM_MIN_CHANNELS 4 -#define PPM_MAX_CHANNELS 12 -static uint16_t ppm_buffer[PPM_MAX_CHANNELS]; -static unsigned ppm_decoded_channels; - -static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS]; - -/* PPM decoder state machine */ -static struct { - uint16_t last_edge; /* last capture time */ - uint16_t last_mark; /* last significant edge */ - unsigned next_channel; - enum { - UNSYNCH = 0, - ARM, - ACTIVE, - INACTIVE - } phase; -} ppm; - -/* last time we got good data */ -static hrt_abstime ppm_timestamp; - -#ifndef CONFIG_DISABLE_MQUEUE -/* message queue we advertise PPM data on */ -static mqd_t ppm_message_queue; -#endif - -/* set if PPM data has not been read */ -static bool ppm_fresh_data; - -/* PPM device node file ops */ - -static int ppm_read(struct file *filp, char *buffer, size_t len); -static int ppm_ioctl(struct file *filp, int cmd, unsigned long arg); - -static const struct file_operations ppm_fops = { - .read = ppm_read, - .ioctl = ppm_ioctl -}; - -/* - * Initialise the PPM system for client use. - */ -int -ppm_input_init(const char *mq_name) -{ - int err; - - /* configure the PPM input pin */ - stm32_configgpio(GPIO_PPM_IN); - - /* and register the device node */ - if (OK != (err = register_driver(PPM_DEVICE_NODE, &ppm_fops, 0666, NULL))) - return err; - -#ifndef CONFIG_DISABLE_MQUEUE - if (mq_name != NULL) { - /* create the message queue */ - struct mq_attr attr = { - .mq_maxmsg = 1, - .mq_msgsize = sizeof(ppm_buffer) - }; - ppm_message_queue = mq_open(mq_name, O_WRONLY | O_CREAT | O_NONBLOCK, 0666, &attr); - if (ppm_message_queue < 0) - return -errno; - } -#endif - - return OK; -} - -/* - * Handle the PPM decoder state machine. - */ -void -ppm_input_decode(bool reset, uint16_t count) -{ - uint16_t width; - uint16_t interval; - unsigned i; - - /* if we missed an edge, we have to give up */ - if (reset) - goto error; - - /* how long since the last edge? */ - width = count - ppm.last_edge; - ppm.last_edge = count; - - /* - * If this looks like a start pulse, then push the last set of values - * and reset the state machine. - * - * Note that this is not a "high performance" design; it implies a whole - * frame of latency between the pulses being received and their being - * considered valid. - */ - if (width >= PPM_MIN_START) { - - /* - * If the number of channels changes unexpectedly, we don't want - * to just immediately jump on the new count as it may be a result - * of noise or dropped edges. Instead, take a few frames to settle. - */ - if (ppm.next_channel != ppm_decoded_channels) { - static int new_channel_count; - static int new_channel_holdoff; - - if (new_channel_count != ppm.next_channel) { - /* start the lock counter for the new channel count */ - new_channel_count = ppm.next_channel; - new_channel_holdoff = PPM_CHANNEL_LOCK; - - } else if (new_channel_holdoff > 0) { - /* this frame matched the last one, decrement the lock counter */ - new_channel_holdoff--; - - } else { - /* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */ - ppm_decoded_channels = new_channel_count; - new_channel_count = 0; - } - } else { - /* frame channel count matches expected, let's use it */ - if (ppm.next_channel > PPM_MIN_CHANNELS) { - for (i = 0; i < ppm.next_channel; i++) - ppm_buffer[i] = ppm_temp_buffer[i]; - ppm_timestamp = hrt_absolute_time(); - ppm_fresh_data = true; -#ifndef CONFIG_DISABLE_MQUEUE - /* advertise the new data to the message queue */ - mq_send(ppm_message_queue, ppm_buffer, ppm_decoded_channels * sizeof(ppm_buffer[0]), 0); -#endif - } - } - - /* reset for the next frame */ - ppm.next_channel = 0; - - /* next edge is the reference for the first channel */ - ppm.phase = ARM; - - return; - } - - switch (ppm.phase) { - case UNSYNCH: - /* we are waiting for a start pulse - nothing useful to do here */ - return; - - case ARM: - /* we expect a pulse giving us the first mark */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ - - /* record the mark timing, expect an inactive edge */ - ppm.last_mark = count; - ppm.phase = INACTIVE; - return; - - case INACTIVE: - /* this edge is not interesting, but now we are ready for the next mark */ - ppm.phase = ACTIVE; - - /* note that we don't bother looking at the timing of this edge */ - - return; - - case ACTIVE: - /* we expect a well-formed pulse */ - if (width > PPM_MAX_PULSE_WIDTH) - goto error; /* pulse was too long */ - - /* determine the interval from the last mark */ - interval = count - ppm.last_mark; - ppm.last_mark = count; - - /* if the mark-mark timing is out of bounds, abandon the frame */ - if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE)) - goto error; - - /* if we have room to store the value, do so */ - if (ppm.next_channel < PPM_MAX_CHANNELS) - ppm_temp_buffer[ppm.next_channel++] = interval; - - ppm.phase = INACTIVE; - return; - - } - - /* the state machine is corrupted; reset it */ - -error: - /* we don't like the state of the decoder, reset it and try again */ - ppm.phase = UNSYNCH; - ppm_decoded_channels = 0; -} - -static int -ppm_read(struct file *filp, char *buffer, size_t len) -{ - size_t avail; - - /* the size of the returned data indicates the number of channels */ - avail = ppm_decoded_channels * sizeof(ppm_buffer[0]); - - /* if we have not decoded a frame, that's an I/O error */ - if (avail == 0) - return -EIO; - - /* if the caller's buffer is too small, that's also bad */ - if (len < avail) - return -EFBIG; - - /* if the caller doesn't want to block, and there is no fresh data, that's EWOULDBLOCK */ - if ((filp->f_oflags & O_NONBLOCK) && (!ppm_fresh_data)) - return -EWOULDBLOCK; - - /* - * Return the channel data. - * - * Note that we have to block the HRT while copying to avoid the - * possibility that we'll get interrupted in the middle of copying - * a single value. - */ - irqstate_t flags = irqsave(); - - memcpy(buffer, ppm_buffer, avail); - ppm_fresh_data = false; - - irqrestore(flags); - - return OK; -} - -static int -ppm_ioctl(struct file *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case PPM_INPUT_STATUS: - /* if we have received a frame within the timeout, the signal is "good" */ - if ((hrt_absolute_time() - ppm_timestamp) < PPM_INPUT_TIMEOUT) { - *(ppm_input_status_t *)arg = PPM_STATUS_SIGNAL_CURRENT; - } else { - /* reset the number of channels so that any attempt to read data will fail */ - ppm_decoded_channels = 0; - *(ppm_input_status_t *)arg = PPM_STATUS_NO_SIGNAL; - } - return OK; - - case PPM_INPUT_CHANNELS: - *(ppm_input_channel_count_t *)arg = ppm_decoded_channels; - return OK; - - default: - return -ENOTTY; - } - -} - -#endif /* CONFIG_HRT_PPM */ - - diff --git a/nuttx/configs/px4io/src/drv_pwm_servo.c b/nuttx/configs/px4io/src/drv_pwm_servo.c deleted file mode 100644 index 4d821cba6..000000000 --- a/nuttx/configs/px4io/src/drv_pwm_servo.c +++ /dev/null @@ -1,318 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Servo driver supporting PWM servos connected to STM32 timer blocks. - * - * Works with any of the 'generic' or 'advanced' STM32 timers that - * have output pins, does not require an interrupt. - */ - -#include <nuttx/config.h> -#include <nuttx/arch.h> - -#include <sys/types.h> -#include <stdbool.h> - -#include <debug.h> -#include <errno.h> - -#include <arch/board/board.h> -#include <arch/board/drv_pwm_servo.h> - -#include "stm32_gpio.h" -#include "stm32_tim.h" - -#ifdef CONFIG_PWM_SERVO - -static const struct pwm_servo_config *cfg; - -#define REG(_tmr, _reg) (*(volatile uint32_t *)(cfg->timers[_tmr].base + _reg)) - -#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET) -#define rCR2(_tmr) REG(_tmr, STM32_GTIM_CR2_OFFSET) -#define rSMCR(_tmr) REG(_tmr, STM32_GTIM_SMCR_OFFSET) -#define rDIER(_tmr) REG(_tmr, STM32_GTIM_DIER_OFFSET) -#define rSR(_tmr) REG(_tmr, STM32_GTIM_SR_OFFSET) -#define rEGR(_tmr) REG(_tmr, STM32_GTIM_EGR_OFFSET) -#define rCCMR1(_tmr) REG(_tmr, STM32_GTIM_CCMR1_OFFSET) -#define rCCMR2(_tmr) REG(_tmr, STM32_GTIM_CCMR2_OFFSET) -#define rCCER(_tmr) REG(_tmr, STM32_GTIM_CCER_OFFSET) -#define rCNT(_tmr) REG(_tmr, STM32_GTIM_CNT_OFFSET) -#define rPSC(_tmr) REG(_tmr, STM32_GTIM_PSC_OFFSET) -#define rARR(_tmr) REG(_tmr, STM32_GTIM_ARR_OFFSET) -#define rCCR1(_tmr) REG(_tmr, STM32_GTIM_CCR1_OFFSET) -#define rCCR2(_tmr) REG(_tmr, STM32_GTIM_CCR2_OFFSET) -#define rCCR3(_tmr) REG(_tmr, STM32_GTIM_CCR3_OFFSET) -#define rCCR4(_tmr) REG(_tmr, STM32_GTIM_CCR4_OFFSET) -#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET) -#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET) - -static void -pwm_timer_init(unsigned timer) -{ - /* enable the timer clock before we try to talk to it */ - modifyreg32(cfg->timers[timer].clock_register, 0, cfg->timers[timer].clock_bit); - - /* disable and configure the timer */ - rCR1(timer) = 0; - rCR2(timer) = 0; - rSMCR(timer) = 0; - rDIER(timer) = 0; - rCCER(timer) = 0; - rCCMR1(timer) = 0; - rCCMR2(timer) = 0; - rCCER(timer) = 0; - rDCR(timer) = 0; - - /* configure the timer to free-run at 1MHz */ - rPSC(timer) = (cfg->timers[timer].clock_freq / 1000000) -1; - - /* and update at the desired rate */ - rARR(timer) = (1000000 / cfg->update_rate) - 1; - - /* generate an update event; reloads the counter and all registers */ - rEGR(timer) = GTIM_EGR_UG; - - /* note that the timer is left disabled - arming is performed separately */ -} - -static void -pwm_servos_arm(bool armed) -{ - /* iterate timers and arm/disarm appropriately */ - for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (cfg->timers[i].base != 0) - rCR1(i) = armed ? GTIM_CR1_CEN : 0; - } -} - -static void -pwm_channel_init(unsigned channel) -{ - unsigned timer = cfg->channels[channel].timer_index; - - /* configure the GPIO first */ - stm32_configgpio(cfg->channels[channel].gpio); - - /* configure the channel */ - switch (cfg->channels[channel].timer_channel) { - case 1: - rCCMR1(timer) |= (6 << 4); - rCCR1(timer) = cfg->channels[channel].default_value; - rCCER(timer) |= (1 << 0); - break; - case 2: - rCCMR1(timer) |= (6 << 12); - rCCR2(timer) = cfg->channels[channel].default_value; - rCCER(timer) |= (1 << 4); - break; - case 3: - rCCMR2(timer) |= (6 << 4); - rCCR3(timer) = cfg->channels[channel].default_value; - rCCER(timer) |= (1 << 8); - break; - case 4: - rCCMR2(timer) |= (6 << 12); - rCCR4(timer) = cfg->channels[channel].default_value; - rCCER(timer) |= (1 << 12); - break; - } -} - -static void -pwm_channel_set(unsigned channel, servo_position_t value) -{ - if (channel >= PWM_SERVO_MAX_CHANNELS) { - lldbg("pwm_channel_set: bogus channel %u\n", channel); - return; - } - - unsigned timer = cfg->channels[channel].timer_index; - - /* test timer for validity */ - if ((cfg->timers[timer].base == 0) || - (cfg->channels[channel].gpio == 0)) - return; - - /* configure the channel */ - if (value > 0) - value--; - switch (cfg->channels[channel].timer_channel) { - case 1: - rCCR1(timer) = value; - break; - case 2: - rCCR2(timer) = value; - break; - case 3: - rCCR3(timer) = value; - break; - case 4: - rCCR4(timer) = value; - break; - } -} - -static servo_position_t -pwm_channel_get(unsigned channel) -{ - if (channel >= PWM_SERVO_MAX_CHANNELS) { - lldbg("pwm_channel_get: bogus channel %u\n", channel); - return 0; - } - - unsigned timer = cfg->channels[channel].timer_index; - servo_position_t value = 0; - - /* test timer for validity */ - if ((cfg->timers[timer].base == 0) || - (cfg->channels[channel].gpio == 0)) - return 0; - - /* configure the channel */ - switch (cfg->channels[channel].timer_channel) { - case 1: - value = rCCR1(timer); - break; - case 2: - value = rCCR2(timer); - break; - case 3: - value = rCCR3(timer); - break; - case 4: - value = rCCR4(timer); - break; - } - return value; -} - -static int pwm_servo_write(struct file *filp, const char *buffer, size_t len); -static int pwm_servo_read(struct file *filp, char *buffer, size_t len); -static int pwm_servo_ioctl(struct file *filep, int cmd, unsigned long arg); - -static const struct file_operations pwm_servo_fops = { - .write = pwm_servo_write, - .read = pwm_servo_read, - .ioctl = pwm_servo_ioctl -}; - -static int -pwm_servo_write(struct file *filp, const char *buffer, size_t len) -{ - unsigned channels = len / sizeof(servo_position_t); - servo_position_t *pdata = (servo_position_t *)buffer; - unsigned i; - - if (channels > PWM_SERVO_MAX_CHANNELS) - return -EIO; - - for (i = 0; i < channels; i++) - pwm_channel_set(i, pdata[i]); - - return i * sizeof(servo_position_t); -} - -static int -pwm_servo_read(struct file *filp, char *buffer, size_t len) -{ - unsigned channels = len / sizeof(servo_position_t); - servo_position_t *pdata = (servo_position_t *)buffer; - unsigned i; - - if (channels > PWM_SERVO_MAX_CHANNELS) - return -EIO; - - for (i = 0; i < channels; i++) - pdata[i] = pwm_channel_get(i); - - return i * sizeof(servo_position_t); -} - -static int -pwm_servo_ioctl(struct file *filep, int cmd, unsigned long arg) -{ - /* regular ioctl? */ - switch (cmd) { - case PWM_SERVO_ARM: - pwm_servos_arm(true); - return 0; - - case PWM_SERVO_DISARM: - pwm_servos_arm(false); - return 0; - } - - /* channel set? */ - if ((cmd >= PWM_SERVO_SET(0)) && (cmd < PWM_SERVO_SET(PWM_SERVO_MAX_CHANNELS))) { - /* XXX sanity-check value? */ - pwm_channel_set(cmd - PWM_SERVO_SET(0), (servo_position_t)arg); - return 0; - } - - /* channel get? */ - if ((cmd >= PWM_SERVO_GET(0)) && (cmd < PWM_SERVO_GET(PWM_SERVO_MAX_CHANNELS))) { - /* XXX sanity-check value? */ - *(servo_position_t *)arg = pwm_channel_get(cmd - PWM_SERVO_GET(0)); - return 0; - } - - /* not a recognised value */ - return -ENOTTY; -} - - -int -pwm_servo_init(const struct pwm_servo_config *config) -{ - /* save a pointer to the configuration */ - cfg = config; - - /* do basic timer initialisation first */ - for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) { - if (cfg->timers[i].base != 0) - pwm_timer_init(i); - } - - /* now init channels */ - for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) { - if (cfg->channels[i].gpio != 0) - pwm_channel_init(i); - } - - /* register the device */ - return register_driver("/dev/pwm_servo", &pwm_servo_fops, 0666, NULL); -} - -#endif /* CONFIG_PWM_SERVO */
\ No newline at end of file diff --git a/nuttx/configs/px4io/src/empty.c b/nuttx/configs/px4io/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx/configs/px4io/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/nuttx/configs/px4io/src/up_adc.c b/nuttx/configs/px4io/src/up_adc.c deleted file mode 100644 index c19f57f96..000000000 --- a/nuttx/configs/px4io/src/up_adc.c +++ /dev/null @@ -1,164 +0,0 @@ -/************************************************************************************ - * configs/stm3210e-eval/src/up_adc.c - * arch/arm/src/board/up_adc.c - * - * Copyright (C) 2011-2012 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <gnutt@nuttx.org> - * - * 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 NuttX 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. - * - ************************************************************************************/ - -/************************************************************************************ - * Included Files - ************************************************************************************/ - -#include <nuttx/config.h> - -#include <errno.h> -#include <debug.h> - -#include <nuttx/analog/adc.h> -#include <arch/board/board.h> - -#include "chip.h" -#include "up_arch.h" - -#include "stm32_pwm.h" -#include "px4io-internal.h" - -#ifdef CONFIG_ADC - -/************************************************************************************ - * Definitions - ************************************************************************************/ -/* Configuration ********************************************************************/ -/* Up to 3 ADC interfaces are supported */ - -#if STM32_NADC < 3 -# undef CONFIG_STM32_ADC3 -#endif - -#if STM32_NADC < 2 -# undef CONFIG_STM32_ADC2 -#endif - -#if STM32_NADC < 1 -# undef CONFIG_STM32_ADC1 -#endif - -#if defined(CONFIG_STM32_ADC1) || defined(CONFIG_STM32_ADC2) || defined(CONFIG_STM32_ADC3) -#ifndef CONFIG_STM32_ADC1 -# warning "Channel information only available for ADC1" -#endif - -/* The number of ADC channels in the conversion list */ - -#define ADC1_NCHANNELS 2 - -/************************************************************************************ - * Private Data - ************************************************************************************/ - -/* Identifying number of each ADC channel: Variable Resistor */ - -#ifdef CONFIG_STM32_ADC1 -static const uint8_t g_chanlist[ADC1_NCHANNELS] = {4, 5}; - -/* Configurations of pins used byte each ADC channels */ - -static const uint32_t g_pinlist[ADC1_NCHANNELS] = {GPIO_ADC1_IN4, GPIO_ADC1_IN5}; -#endif - -/************************************************************************************ - * Private Functions - ************************************************************************************/ - -/************************************************************************************ - * Public Functions - ************************************************************************************/ - -/************************************************************************************ - * Name: adc_devinit - * - * Description: - * All STM32 architectures must provide the following interface to work with - * examples/adc. - * - ************************************************************************************/ - -int adc_devinit(void) -{ -#ifdef CONFIG_STM32_ADC1 - static bool initialized = false; - struct adc_dev_s *adc; - int ret; - int i; - - /* Check if we have already initialized */ - - if (!initialized) - { - /* Configure the pins as analog inputs for the selected channels */ - - for (i = 0; i < ADC1_NCHANNELS; i++) - { - stm32_configgpio(g_pinlist[i]); - } - - /* Call stm32_adcinitialize() to get an instance of the ADC interface */ - - adc = stm32_adcinitialize(1, g_chanlist, ADC1_NCHANNELS); - if (adc == NULL) - { - adbg("ERROR: Failed to get ADC interface\n"); - return -ENODEV; - } - - /* Register the ADC driver at "/dev/adc0" */ - - ret = adc_register("/dev/adc0", adc); - if (ret < 0) - { - adbg("adc_register failed: %d\n", ret); - return ret; - } - - /* Now we are initialized */ - - initialized = true; - } - - return OK; -#else - return -ENOSYS; -#endif -} - -#endif /* CONFIG_STM32_ADC1 || CONFIG_STM32_ADC2 || CONFIG_STM32_ADC3 */ -#endif /* CONFIG_ADC */ diff --git a/nuttx/configs/px4io/src/up_boardinitialize.c b/nuttx/configs/px4io/src/up_boardinitialize.c deleted file mode 100644 index f6900ebb5..000000000 --- a/nuttx/configs/px4io/src/up_boardinitialize.c +++ /dev/null @@ -1,178 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 Board initialisation and configuration data. - */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include <nuttx/config.h> - -#include <stdbool.h> -#include <stdio.h> -#include <debug.h> -#include <errno.h> - -#include <arch/board/board.h> -#include <arch/board/up_boardinitialize.h> -#include <arch/board/up_hrt.h> -#include <arch/board/drv_pwm_servo.h> -#include <arch/board/drv_gpio.h> - -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" - -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" - -/**************************************************************************** - * Pre-Processor Definitions - ****************************************************************************/ - -/* Debug ********************************************************************/ - -/* Configuration ************************************************************/ - -#if CONFIG_PWM_SERVO - /* - * Servo configuration for the PX4IO board. - */ - static const struct pwm_servo_config servo_config = { - .update_rate = 50, - .timers = { - { - .base = STM32_TIM2_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM2EN, - .clock_freq = STM32_APB1_TIM2_CLKIN - }, - { - .base = STM32_TIM3_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM3EN, - .clock_freq = STM32_APB1_TIM3_CLKIN - }, - { - .base = STM32_TIM4_BASE, - .clock_register = STM32_RCC_APB1ENR, - .clock_bit = RCC_APB1ENR_TIM4EN, - .clock_freq = STM32_APB1_TIM4_CLKIN - }, - }, - .channels = { - { - .gpio = GPIO_TIM2_CH1OUT, - .timer_index = 0, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM2_CH2OUT, - .timer_index = 0, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH3OUT, - .timer_index = 2, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM4_CH4OUT, - .timer_index = 2, - .timer_channel = 4, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH1OUT, - .timer_index = 1, - .timer_channel = 1, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH2OUT, - .timer_index = 1, - .timer_channel = 2, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH3OUT, - .timer_index = 1, - .timer_channel = 3, - .default_value = 1000, - }, - { - .gpio = GPIO_TIM3_CH4OUT, - .timer_index = 1, - .timer_channel = 4, - .default_value = 1000, - }, - } - }; -#endif - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - -/**************************************************************************** - * Name: nsh_archinitialize - * - * Description: - * Perform architecture specific initialization - * - ****************************************************************************/ - -int up_boardinitialize() -{ - /* configure the high-resolution time/callout interface */ -#ifdef CONFIG_HRT_TIMER - hrt_init(CONFIG_HRT_TIMER); -#endif - - /* configure the PWM servo driver */ -#if CONFIG_PWM_SERVO - pwm_servo_init(&servo_config); -#endif - - /* configure the GPIO driver */ - gpio_drv_init(); - - return OK; -} diff --git a/nuttx/configs/px4io/src/up_hrt.c b/nuttx/configs/px4io/src/up_hrt.c deleted file mode 100644 index d0c46bd26..000000000 --- a/nuttx/configs/px4io/src/up_hrt.c +++ /dev/null @@ -1,664 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 High-resolution timer callouts and timekeeping. - * - * This can use any general or advanced STM32 timer. - * - * Note that really, this could use systick too, but that's - * monopolised by NuttX and stealing it would just be awkward. - * - * We don't use the NuttX STM32 driver per se; rather, we - * claim the timer and then drive it directly. - */ - -#include <nuttx/config.h> -#include <nuttx/arch.h> -#include <nuttx/irq.h> - -#include <sys/types.h> -#include <stdbool.h> - -#include <assert.h> -#include <debug.h> -#include <time.h> -#include <queue.h> -#include <errno.h> -#include <string.h> - -#include <arch/board/board.h> -#include <arch/board/up_hrt.h> - -#include "chip.h" -#include "up_internal.h" -#include "up_arch.h" - -#include "stm32_internal.h" -#include "stm32_gpio.h" -#include "stm32_tim.h" - -#ifdef CONFIG_HRT_TIMER - -/* HRT configuration */ -#if HRT_TIMER == 1 -# define HRT_TIMER_BASE STM32_TIM1_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM1EN -# define HRT_TIMER_VECTOR STM32_IRQ_TIM1CC -# define HRT_TIMER_CLOCK STM32_APB2_TIM1_CLKIN -# if CONFIG_STM32_TIM1 -# error must not set CONFIG_STM32_TIM1=y and HRT_TIMER=1 -# endif -#elif HRT_TIMER == 2 -# define HRT_TIMER_BASE STM32_TIM2_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM2EN -# define HRT_TIMER_VECTOR STM32_IRQ_TIM2 -# define HRT_TIMER_CLOCK STM32_APB1_TIM2_CLKIN -# if CONFIG_STM32_TIM2 -# error must not set CONFIG_STM32_TIM2=y and HRT_TIMER=2 -# endif -#elif HRT_TIMER == 3 -# define HRT_TIMER_BASE STM32_TIM3_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN -# define HRT_TIMER_VECTOR STM32_IRQ_TIM3 -# define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN -# if CONFIG_STM32_TIM3 -# error must not set CONFIG_STM32_TIM3=y and HRT_TIMER=3 -# endif -#elif HRT_TIMER == 4 -# define HRT_TIMER_BASE STM32_TIM4_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM4EN -# define HRT_TIMER_VECTOR STM32_IRQ_TIM4 -# define HRT_TIMER_CLOCK STM32_APB1_TIM4_CLKIN -# if CONFIG_STM32_TIM4 -# error must not set CONFIG_STM32_TIM4=y and HRT_TIMER=4 -# endif -#elif HRT_TIMER == 5 -# define HRT_TIMER_BASE STM32_TIM5_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM5EN -# define HRT_TIMER_VECTOR STM32_IRQ_TIM5 -# define HRT_TIMER_CLOCK STM32_APB1_TIM5_CLKIN -# if CONFIG_STM32_TIM5 -# error must not set CONFIG_STM32_TIM5=y and HRT_TIMER=5 -# endif -#elif HRT_TIMER == 8 -# define HRT_TIMER_BASE STM32_TIM8_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB2ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM8EN -# define HRT_TIMER_VECTOR STM32_IRQ_TIM8CC -# define HRT_TIMER_CLOCK STM32_APB2_TIM8_CLKIN -# if CONFIG_STM32_TIM8 -# error must not set CONFIG_STM32_TIM8=y and HRT_TIMER=6 -# endif -#elif HRT_TIMER == 9 -# define HRT_TIMER_BASE STM32_TIM9_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM9EN -# define HRT_TIMER_VECTOR STM32_IRQ_TIM1BRK -# define HRT_TIMER_CLOCK STM32_APB1_TIM9_CLKIN -# if CONFIG_STM32_TIM9 -# error must not set CONFIG_STM32_TIM9=y and HRT_TIMER=9 -# endif -#elif HRT_TIMER == 10 -# define HRT_TIMER_BASE STM32_TIM10_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN -# define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP -# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN -# if CONFIG_STM32_TIM10 -# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10 -# endif -#elif HRT_TIMER == 11 -# define HRT_TIMER_BASE STM32_TIM11_BASE -# define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN -# define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM -# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN -# if CONFIG_STM32_TIM11 -# error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 -# endif -#else -# error HRT_TIMER must be set in board.h if CONFIG_HRT_TIMER=y -#endif - -/* - * HRT clock must be a multiple of 1MHz greater than 1MHz - */ -#if (HRT_TIMER_CLOCK % 1000000) != 0 -# error HRT_TIMER_CLOCK must be a multiple of 1MHz -#endif -#if HRT_TIMER_CLOCK <= 1000000 -# error HRT_TIMER_CLOCK must be greater than 1MHz -#endif - -/* - * Minimum/maximum deadlines. - * - * These are suitable for use with a 16-bit timer/counter clocked - * at 1MHz. The high-resolution timer need only guarantee that it - * not wrap more than once in the 50ms period for absolute time to - * be consistently maintained. - * - * The minimum deadline must be such that the time taken between - * reading a time and writing a deadline to the timer cannot - * result in missing the deadline. - */ -#define HRT_INTERVAL_MIN 50 -#define HRT_INTERVAL_MAX 50000 - -/* - * Period of the free-running counter, in microseconds. - */ -#define HRT_COUNTER_PERIOD 65536 - -/* - * Scaling factor(s) for the free-running counter; convert an input - * in counts to a time in microseconds. - */ -#define HRT_COUNTER_SCALE(_c) (_c) - -/* - * Timer register accessors - */ -#define REG(_reg) (*(volatile uint32_t *)(HRT_TIMER_BASE + _reg)) - -#define rCR1 REG(STM32_GTIM_CR1_OFFSET) -#define rCR2 REG(STM32_GTIM_CR2_OFFSET) -#define rSMCR REG(STM32_GTIM_SMCR_OFFSET) -#define rDIER REG(STM32_GTIM_DIER_OFFSET) -#define rSR REG(STM32_GTIM_SR_OFFSET) -#define rEGR REG(STM32_GTIM_EGR_OFFSET) -#define rCCMR1 REG(STM32_GTIM_CCMR1_OFFSET) -#define rCCMR2 REG(STM32_GTIM_CCMR2_OFFSET) -#define rCCER REG(STM32_GTIM_CCER_OFFSET) -#define rCNT REG(STM32_GTIM_CNT_OFFSET) -#define rPSC REG(STM32_GTIM_PSC_OFFSET) -#define rARR REG(STM32_GTIM_ARR_OFFSET) -#define rCCR1 REG(STM32_GTIM_CCR1_OFFSET) -#define rCCR2 REG(STM32_GTIM_CCR2_OFFSET) -#define rCCR3 REG(STM32_GTIM_CCR3_OFFSET) -#define rCCR4 REG(STM32_GTIM_CCR4_OFFSET) -#define rDCR REG(STM32_GTIM_DCR_OFFSET) -#define rDMAR REG(STM32_GTIM_DMAR_OFFSET) - -/* - * Specific registers and bits used by HRT sub-functions - */ -#if HRT_TIMER_CHANNEL == 1 -# define rCCR_HRT rCCR1 /* compare register for HRT */ -# define DIER_HRT GTIM_DIER_CC1IE /* interrupt enable for HRT */ -# define SR_INT_HRT GTIM_SR_CC1IF /* interrupt status for HRT */ -#elif HRT_TIMER_CHANNEL == 2 -# define rCCR_HRT rCCR2 /* compare register for HRT */ -# define DIER_HRT GTIM_DIER_CC2IE /* interrupt enable for HRT */ -# define SR_INT_HRT GTIM_SR_CC2IF /* interrupt status for HRT */ -#elif HRT_TIMER_CHANNEL == 3 -# define rCCR_HRT rCCR3 /* compare register for HRT */ -# define DIER_HRT GTIM_DIER_CC3IE /* interrupt enable for HRT */ -# define SR_INT_HRT GTIM_SR_CC3IF /* interrupt status for HRT */ -#elif HRT_TIMER_CHANNEL == 4 -# define rCCR_HRT rCCR4 /* compare register for HRT */ -# define DIER_HRT GTIM_DIER_CC4IE /* interrupt enable for HRT */ -# define SR_INT_HRT GTIM_SR_CC4IF /* interrupt status for HRT */ -#else -# error HRT_TIMER_CHANNEL must be a value between 1 and 4 -#endif - -/* - * Queue of callout entries. - */ -static struct sq_queue_s callout_queue; - -/* - * The time corresponding to a counter value of zero, as of the - * last time that hrt_absolute_time() was called. - */ -static hrt_abstime base_time; - -/* timer-specific functions */ -static void hrt_tim_init(int timer); -static int hrt_tim_isr(int irq, void *context); - -/* callout list manipulation */ -static void hrt_call_enter(struct hrt_call *entry); -static void hrt_call_reschedule(void); -static void hrt_call_invoke(void); - -/* - * Specific registers and bits used by PPM sub-functions - */ -#ifdef CONFIG_HRT_PPM -# include <arch/board/drv_ppm_input.h> - -/* - * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it. - */ -# ifndef GTIM_CCER_CC1NP -# define GTIM_CCER_CC1NP 0 -# define GTIM_CCER_CC2NP 0 -# define GTIM_CCER_CC3NP 0 -# define GTIM_CCER_CC4NP 0 -# define PPM_EDGE_FLIP -# endif - -# if HRT_PPM_CHANNEL == 1 -# define rCCR_PPM rCCR1 /* capture register for PPM */ -# define DIER_PPM GTIM_DIER_CC1IE /* capture interrupt (non-DMA mode) */ -# define SR_INT_PPM GTIM_SR_CC1IF /* capture interrupt (non-DMA mode) */ -# define SR_OVF_PPM GTIM_SR_CC1OF /* capture overflow (non-DMA mode) */ -# define CCMR1_PPM 1 /* on TI1 */ -# define CCMR2_PPM 0 -# define CCER_PPM (GTIM_CCER_CC1E | GTIM_CCER_CC1P | GTIM_CCER_CC1NP) /* CC1, both edges */ -# define CCER_PPM_FLIP GTIM_CCER_CC1P -# elif HRT_PPM_CHANNEL == 2 -# define rCCR_PPM rCCR2 /* capture register for PPM */ -# define DIER_PPM GTIM_DIER_CC2IE /* capture interrupt (non-DMA mode) */ -# define SR_INT_PPM GTIM_SR_CC2IF /* capture interrupt (non-DMA mode) */ -# define SR_OVF_PPM GTIM_SR_CC2OF /* capture overflow (non-DMA mode) */ -# define CCMR1_PPM 2 /* on TI2 */ -# define CCMR2_PPM 0 -# define CCER_PPM (GTIM_CCER_CC2E | GTIM_CCER_CC2P | GTIM_CCER_CC2NP) /* CC2, both edges */ -# define CCER_PPM_FLIP GTIM_CCER_CC2P -# elif HRT_PPM_CHANNEL == 3 -# define rCCR_PPM rCCR3 /* capture register for PPM */ -# define DIER_PPM GTIM_DIER_CC3IE /* capture interrupt (non-DMA mode) */ -# define SR_INT_PPM GTIM_SR_CC3IF /* capture interrupt (non-DMA mode) */ -# define SR_OVF_PPM GTIM_SR_CC3OF /* capture overflow (non-DMA mode) */ -# define CCMR1_PPM 0 -# define CCMR2_PPM 1 /* on TI3 */ -# define CCER_PPM (GTIM_CCER_CC3E | GTIM_CCER_CC3P | GTIM_CCER_CC3NP) /* CC3, both edges */ -# define CCER_PPM_FLIP GTIM_CCER_CC3P -# elif HRT_PPM_CHANNEL == 4 -# define rCCR_PPM rCCR4 /* capture register for PPM */ -# define DIER_PPM GTIM_DIER_CC4IE /* capture interrupt (non-DMA mode) */ -# define SR_INT_PPM GTIM_SR_CC4IF /* capture interrupt (non-DMA mode) */ -# define SR_OVF_PPM GTIM_SR_CC4OF /* capture overflow (non-DMA mode) */ -# define CCMR1_PPM 0 -# define CCMR2_PPM 2 /* on TI4 */ -# define CCER_PPM (GTIM_CCER_CC4E | GTIM_CCER_CC4P | GTIM_CCER_CC4NP) /* CC4, both edges */ -# define CCER_PPM_FLIP GTIM_CCER_CC4P -# else -# error HRT_PPM_CHANNEL must be a value between 1 and 4 if CONFIG_HRT_PPM is set -# endif -#else -/* disable the PPM configuration */ -# define rCCR_PPM 0 -# define DIER_PPM 0 -# define SR_INT_PPM 0 -# define SR_OVF_PPM 0 -# define CCMR1_PPM 0 -# define CCMR2_PPM 0 -# define CCER_PPM 0 -#endif /* CONFIG_HRT_PPM */ - -/* - * Initialise the timer we are going to use. - * - * We expect that we'll own one of the reduced-function STM32 general - * timers, and that we can use channel 1 in compare mode. - */ -static void -hrt_tim_init(int timer) -{ - /* clock/power on our timer */ - modifyreg32(HRT_TIMER_POWER_REG, 0, HRT_TIMER_POWER_BIT); - - /* claim our interrupt vector */ - irq_attach(HRT_TIMER_VECTOR, hrt_tim_isr); - - /* disable and configure the timer */ - rCR1 = 0; - rCR2 = 0; - rSMCR = 0; - rDIER = DIER_HRT | DIER_PPM; - rCCER = 0; /* unlock CCMR* registers */ - rCCMR1 = CCMR1_PPM; - rCCMR2 = CCMR2_PPM; - rCCER = CCER_PPM; - rDCR = 0; - - /* configure the timer to free-run at 1MHz */ - rPSC = (HRT_TIMER_CLOCK / 1000000) - 1; /* this really only works for whole-MHz clocks */ - - /* run the full span of the counter */ - rARR = 0xffff; - - /* set an initial capture a little ways off */ - rCCR_HRT = 1000; - - /* generate an update event; reloads the counter, all registers */ - rEGR = GTIM_EGR_UG; - - /* enable the timer */ - rCR1 = GTIM_CR1_CEN; - - /* enable interrupts */ - up_enable_irq(HRT_TIMER_VECTOR); -} - -/* - * Handle the compare interupt by calling the callout dispatcher - * and then re-scheduling the next deadline. - */ -static int -hrt_tim_isr(int irq, void *context) -{ - uint32_t status; - - /* copy interrupt status */ - status = rSR; - - /* ack the interrupts we just read */ - rSR = ~status; - -#ifdef CONFIG_HRT_PPM - /* was this a PPM edge? */ - if (status & (SR_INT_PPM | SR_OVF_PPM)) { - - /* if required, flip edge sensitivity */ -# ifdef PPM_EDGE_FLIP - rCCER ^= CCER_PPM_FLIP; -# endif - - /* feed the edge to the PPM decoder */ - ppm_input_decode(status & SR_OVF_PPM, rCCR_PPM); - } -#endif - - /* was this a timer tick? */ - if (status & SR_INT_HRT) { - /* run any callouts that have met their deadline */ - hrt_call_invoke(); - - /* and schedule the next interrupt */ - hrt_call_reschedule(); - } - - return OK; -} - -/* - * Fetch a never-wrapping absolute time value in microseconds from - * some arbitrary epoch shortly after system start. - */ -hrt_abstime -hrt_absolute_time(void) -{ - static uint32_t last_count; - uint32_t count; - uint32_t flags = irqsave(); - - count = rCNT; - - //lldbg("count %u last_count %u\n", count, last_count); - - /* This simple test is made possible by the guarantee that - * we are always called at least once per counter period. - */ - if (count < last_count) - base_time += HRT_COUNTER_PERIOD; - - last_count = count; - - irqrestore(flags); - - return HRT_COUNTER_SCALE(base_time + count); -} - -/* - * Convert a timespec to absolute time - */ -hrt_abstime -ts_to_abstime(struct timespec *ts) -{ - hrt_abstime result; - - result = (hrt_abstime)(ts->tv_sec) * 1000000; - result += ts->tv_nsec / 1000; - - return result; -} - -/* - * Convert absolute time to a timespec. - */ -void -abstime_to_ts(struct timespec *ts, hrt_abstime abstime) -{ - ts->tv_sec = abstime / 1000000; - abstime -= ts->tv_sec * 1000000; - ts->tv_nsec = abstime * 1000; -} - -/* - * Initalise the high-resolution timing module. - */ -void -hrt_init(int timer) -{ - sq_init(&callout_queue); - hrt_tim_init(timer); -} - -/* - * Call callout(arg) after interval has elapsed. - */ -void -hrt_call_after(struct hrt_call *entry, hrt_abstime delay, hrt_callout callout, void *arg) -{ - entry->deadline = hrt_absolute_time() + delay; - entry->period = 0; - entry->callout = callout; - entry->arg = arg; - - hrt_call_enter(entry); -} - -/* - * Call callout(arg) at calltime. - */ -void -hrt_call_at(struct hrt_call *entry, hrt_abstime calltime, hrt_callout callout, void *arg) -{ - entry->deadline = calltime; - entry->period = 0; - entry->callout = callout; - entry->arg = arg; - - hrt_call_enter(entry); -} - -/* - * Call callout(arg) every period. - */ -void -hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg) -{ - entry->deadline = hrt_absolute_time() + delay; - entry->period = interval; - entry->callout = callout; - entry->arg = arg; - - hrt_call_enter(entry); -} - -/* - * If this returns true, the call has been invoked and removed from the callout list. - * - * Always returns false for repeating callouts. - */ -bool -hrt_called(struct hrt_call *entry) -{ - bool result; - - irqstate_t flags = irqsave(); - result = (entry->deadline == 0); - irqrestore(flags); - - return result; -} - -/* - * Remove the entry from the callout list. - */ -void -hrt_cancel(struct hrt_call *entry) -{ - irqstate_t flags = irqsave(); - - sq_rem(&entry->link, &callout_queue); - entry->deadline = 0; - - /* if this is a periodic call being removed by the callout, prevent it from - * being re-entered when the callout returns. - */ - entry->period = 0; - - irqrestore(flags); -} - -static void -hrt_call_enter(struct hrt_call *entry) -{ - irqstate_t flags = irqsave(); - struct hrt_call *call, *next; - - call = (struct hrt_call *)sq_peek(&callout_queue); - - if ((call == NULL) || (entry->deadline < call->deadline)) { - sq_addfirst(&entry->link, &callout_queue); - //lldbg("call enter at head, reschedule\n"); - /* we changed the next deadline, reschedule the timer event */ - hrt_call_reschedule(); - } else { - do { - next = (struct hrt_call *)sq_next(&call->link); - if ((next == NULL) || (entry->deadline < next->deadline)) { - //lldbg("call enter after head\n"); - sq_addafter(&call->link, &entry->link, &callout_queue); - break; - } - } while ((call = next) != NULL); - } - - //lldbg("scheduled\n"); - irqrestore(flags); -} - -static void -hrt_call_invoke(void) -{ - struct hrt_call *call; - hrt_abstime deadline; - - while (true) { - /* get the current time */ - hrt_abstime now = hrt_absolute_time(); - - call = (struct hrt_call *)sq_peek(&callout_queue); - if (call == NULL) - break; - if (call->deadline > now) - break; - - sq_rem(&call->link, &callout_queue); - //lldbg("call pop\n"); - - /* save the intended deadline for periodic calls */ - deadline = call->deadline; - - /* zero the deadline, as the call has occurred */ - call->deadline = 0; - - /* invoke the callout (if there is one) */ - if (call->callout) { - //lldbg("call %p: %p(%p)\n", call, call->callout, call->arg); - call->callout(call->arg); - } - - /* if the callout has a non-zero period, it has to be re-entered */ - if (call->period != 0) { - call->deadline = deadline + call->period; - hrt_call_enter(call); - } - } -} - -/* - * Reschedule the next timer interrupt. - * - * This routine must be called with interrupts disabled. - */ -static void -hrt_call_reschedule() -{ - hrt_abstime now = hrt_absolute_time(); - struct hrt_call *next = (struct hrt_call *)sq_peek(&callout_queue); - hrt_abstime deadline = now + HRT_INTERVAL_MAX; - - /* - * Determine what the next deadline will be. - * - * Note that we ensure that this will be within the counter - * period, so that when we truncate all but the low 16 bits - * the next time the compare matches it will be the deadline - * we want. - * - * It is important for accurate timekeeping that the compare - * interrupt fires sufficiently often that the base_time update in - * hrt_absolute_time runs at least once per timer period. - */ - if (next != NULL) { - //lldbg("entry in queue\n"); - if (next->deadline <= (now + HRT_INTERVAL_MIN)) { - //lldbg("pre-expired\n"); - /* set a minimal deadline so that we call ASAP */ - deadline = now + HRT_INTERVAL_MIN; - } else if (next->deadline < deadline) { - //lldbg("due soon\n"); - deadline = next->deadline; - } - } - //lldbg("schedule for %u at %u\n", (unsigned)(deadline & 0xffffffff), (unsigned)(now & 0xffffffff)); - - /* set the new compare value */ - rCCR_HRT = deadline & 0xffff; -} - -#endif /* CONFIG_HRT_TIMER */ diff --git a/nuttx/configs/px4io/src/up_nsh.c b/nuttx/configs/px4io/src/up_nsh.c deleted file mode 100644 index 035838780..000000000 --- a/nuttx/configs/px4io/src/up_nsh.c +++ /dev/null @@ -1,63 +0,0 @@ -/**************************************************************************** - * config/stm3210e_eval/src/up_nsh.c - * arch/arm/src/board/up_nsh.c - * - * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. - * Author: Gregory Nutt <spudmonkey@racsa.co.cr> - * - * 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 NuttX 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. - * - ****************************************************************************/ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include <nuttx/config.h> - -#include <stdbool.h> -#include <stdio.h> -#include <debug.h> -#include <errno.h> - -#include "stm32_internal.h" - -#include <arch/board/up_boardinitialize.h> - -/**************************************************************************** - * Name: nsh_archinitialize - * - * Description: - * Perform architecture specific initialization - * - ****************************************************************************/ - -int nsh_archinitialize(void) -{ - return up_boardinitialize(); -} diff --git a/nuttx/drivers/serial/serial.c b/nuttx/drivers/serial/serial.c index b289bb80b..40011199b 100644 --- a/nuttx/drivers/serial/serial.c +++ b/nuttx/drivers/serial/serial.c @@ -328,7 +328,7 @@ static ssize_t uart_write(FAR struct file *filep, FAR const char *buffer, size_t /* If the ONLCR flag is set, we should translate \n to \r\n */ ret = OK; - if ((ch == '\n') && (dev->termios_s.c_oflag && ONLCR)) + if ((ch == '\n') && (dev->termios_s.c_oflag & ONLCR)) { ret = uart_putxmitchar(dev, '\r'); } |