diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 513ef7157..a83ec4122 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -211,6 +211,7 @@ SRC += $(PIOSCOMMON)/pios_com.c SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_iap.c SRC += $(PIOSCOMMON)/pios_bl_helper.c +SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/printf-stdarg.c ## Libraries for flight calculations SRC += $(FLIGHTLIB)/fifo_buffer.c diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 4d9fee769..0f243b7cf 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -1,116 +1,118 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotCore OpenPilot Core - * @{ - * - * @file pios_config.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief PiOS configuration header. - * Central compile time config for the project. - * In particular, pios_config.h is where you define which PiOS libraries - * and features are included in the firmware. - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#ifndef PIOS_CONFIG_H -#define PIOS_CONFIG_H - -/* Enable/Disable PiOS Modules */ -#define PIOS_INCLUDE_ADC -#define PIOS_INCLUDE_DELAY -#if defined(USE_I2C) -#define PIOS_INCLUDE_I2C -#define PIOS_INCLUDE_I2C_ESC -#endif -#define PIOS_INCLUDE_IRQ -#define PIOS_INCLUDE_LED - -/* Receiver interfaces - only one allowed */ -#if !defined(USE_SPEKTRUM) && !defined(USE_SBUS) -//#define PIOS_INCLUDE_PPM -#define PIOS_INCLUDE_PWM -#endif - -/* USART-based PIOS modules */ -#if defined(USE_TELEMETRY) -#define PIOS_INCLUDE_TELEMETRY_RF -#endif -#if defined(USE_GPS) -#define PIOS_INCLUDE_GPS -#endif -#if defined(USE_SPEKTRUM) -#define PIOS_INCLUDE_SPEKTRUM -#endif -#if defined(USE_SBUS) -#define PIOS_INCLUDE_SBUS -#endif - -#define PIOS_INCLUDE_SERVO -#define PIOS_INCLUDE_SPI -#define PIOS_INCLUDE_SYS -#define PIOS_INCLUDE_USART -#define PIOS_INCLUDE_USB_HID -#define PIOS_INCLUDE_COM -#define PIOS_INCLUDE_SETTINGS -#define PIOS_INCLUDE_FREERTOS -#define PIOS_INCLUDE_GPIO -#define PIOS_INCLUDE_EXTI -#define PIOS_INCLUDE_RTC -#define PIOS_INCLUDE_WDG -#define PIOS_INCLUDE_BL_HELPER - -#define PIOS_INCLUDE_ADXL345 -#define PIOS_INCLUDE_FLASH - -/* A really shitty setting saving implementation */ -#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS - -/* Defaults for Logging */ -#define LOG_FILENAME "PIOS.LOG" -#define STARTUP_LOG_ENABLED 1 - -/* COM Module */ -#define GPS_BAUDRATE 19200 -#define TELEM_BAUDRATE 19200 -#define AUXUART_ENABLED 0 -#define AUXUART_BAUDRATE 19200 - -/* Alarm Thresholds */ -#define HEAP_LIMIT_WARNING 220 -#define HEAP_LIMIT_CRITICAL 150 -#define CPULOAD_LIMIT_WARNING 80 -#define CPULOAD_LIMIT_CRITICAL 95 - -/* Task stack sizes */ -#define PIOS_ACTUATOR_STACK_SIZE 1020 -#define PIOS_MANUAL_STACK_SIZE 724 -#define PIOS_SYSTEM_STACK_SIZE 560 -#define PIOS_STABILIZATION_STACK_SIZE 524 -#define PIOS_TELEM_STACK_SIZE 500 - -#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 -//#define PIOS_QUATERNION_STABILIZATION - -#endif /* PIOS_CONFIG_H */ -/** - * @} - * @} - */ +/** + ****************************************************************************** + * @addtogroup OpenPilotSystem OpenPilot System + * @{ + * @addtogroup OpenPilotCore OpenPilot Core + * @{ + * + * @file pios_config.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief PiOS configuration header. + * Central compile time config for the project. + * In particular, pios_config.h is where you define which PiOS libraries + * and features are included in the firmware. + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#ifndef PIOS_CONFIG_H +#define PIOS_CONFIG_H + +/* Enable/Disable PiOS Modules */ +#define PIOS_INCLUDE_ADC +#define PIOS_INCLUDE_DELAY +#if defined(USE_I2C) +#define PIOS_INCLUDE_I2C +#define PIOS_INCLUDE_I2C_ESC +#endif +#define PIOS_INCLUDE_IRQ +#define PIOS_INCLUDE_LED + +#define PIOS_INCLUDE_RCVR + +/* Receiver interfaces - only one allowed */ +#if !defined(USE_SPEKTRUM) && !defined(USE_SBUS) +//#define PIOS_INCLUDE_PPM +#define PIOS_INCLUDE_PWM +#endif + +/* USART-based PIOS modules */ +#if defined(USE_TELEMETRY) +#define PIOS_INCLUDE_TELEMETRY_RF +#endif +#if defined(USE_GPS) +#define PIOS_INCLUDE_GPS +#endif +#if defined(USE_SPEKTRUM) +#define PIOS_INCLUDE_SPEKTRUM +#endif +#if defined(USE_SBUS) +#define PIOS_INCLUDE_SBUS +#endif + +#define PIOS_INCLUDE_SERVO +#define PIOS_INCLUDE_SPI +#define PIOS_INCLUDE_SYS +#define PIOS_INCLUDE_USART +#define PIOS_INCLUDE_USB_HID +#define PIOS_INCLUDE_COM +#define PIOS_INCLUDE_SETTINGS +#define PIOS_INCLUDE_FREERTOS +#define PIOS_INCLUDE_GPIO +#define PIOS_INCLUDE_EXTI +#define PIOS_INCLUDE_RTC +#define PIOS_INCLUDE_WDG +#define PIOS_INCLUDE_BL_HELPER + +#define PIOS_INCLUDE_ADXL345 +#define PIOS_INCLUDE_FLASH + +/* A really shitty setting saving implementation */ +#define PIOS_INCLUDE_FLASH_SECTOR_SETTINGS + +/* Defaults for Logging */ +#define LOG_FILENAME "PIOS.LOG" +#define STARTUP_LOG_ENABLED 1 + +/* COM Module */ +#define GPS_BAUDRATE 19200 +#define TELEM_BAUDRATE 19200 +#define AUXUART_ENABLED 0 +#define AUXUART_BAUDRATE 19200 + +/* Alarm Thresholds */ +#define HEAP_LIMIT_WARNING 220 +#define HEAP_LIMIT_CRITICAL 150 +#define CPULOAD_LIMIT_WARNING 80 +#define CPULOAD_LIMIT_CRITICAL 95 + +/* Task stack sizes */ +#define PIOS_ACTUATOR_STACK_SIZE 1020 +#define PIOS_MANUAL_STACK_SIZE 724 +#define PIOS_SYSTEM_STACK_SIZE 560 +#define PIOS_STABILIZATION_STACK_SIZE 524 +#define PIOS_TELEM_STACK_SIZE 500 + +#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998 +//#define PIOS_QUATERNION_STABILIZATION + +#endif /* PIOS_CONFIG_H */ +/** + * @} + * @} + */ diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index cd1e7952a..b724cc806 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -469,13 +469,13 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = { }, }; +#include static uint32_t pios_usart_spektrum_id; void PIOS_USART_spektrum_irq_handler(void) { - SPEKTRUM_IRQHandler(pios_usart_spektrum_id); + PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id); } -#include void RTC_IRQHandler(); void RTC_IRQHandler() __attribute__ ((alias ("PIOS_SUPV_irq_handler"))); const struct pios_spektrum_cfg pios_spektrum_cfg = { @@ -501,7 +501,7 @@ void PIOS_SUPV_irq_handler() { if (RTC_GetITStatus(RTC_IT_SEC)) { /* Call the right handler */ - PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id); + PIOS_SPEKTRUMSV_irq_handler(pios_usart_spektrum_id); /* Wait until last write operation on RTC registers has finished */ RTC_WaitForLastTask(); @@ -561,7 +561,7 @@ const struct pios_usart_cfg pios_usart_sbus_cfg = { static uint32_t pios_usart_sbus_id; void PIOS_USART_sbus_irq_handler(void) { - SBUS_IRQHandler(pios_usart_sbus_id); + PIOS_SBUS_irq_handler(pios_usart_sbus_id); } #include @@ -587,7 +587,7 @@ void PIOS_SUPV_irq_handler() { if (RTC_GetITStatus(RTC_IT_SEC)) { /* Call the right handler */ - PIOS_SBUS_irq_handler(pios_usart_sbus_id); + PIOS_SBUSSV_irq_handler(pios_usart_sbus_id); /* Wait until last write operation on RTC registers has finished */ RTC_WaitForLastTask(); @@ -869,6 +869,13 @@ void PIOS_I2C_main_adapter_er_irq_handler(void) #endif /* PIOS_INCLUDE_I2C */ +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +uint32_t pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_DEVS]; +uint32_t pios_rcvr_max_channel; +#endif /* PIOS_INCLUDE_RCVR */ + extern const struct pios_com_driver pios_usb_com_driver; uint32_t pios_com_telem_rf_id; @@ -897,18 +904,6 @@ void PIOS_Board_Init(void) { PIOS_FLASHFS_Init(); -#if defined(PIOS_INCLUDE_SPEKTRUM) - /* SPEKTRUM init must come before comms */ - PIOS_SPEKTRUM_Init(); - - if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_COM_Init(&pios_com_spektrum_id, &pios_usart_com_driver, pios_usart_spektrum_id)) { - PIOS_DEBUG_Assert(0); - } -#endif - /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -938,6 +933,26 @@ void PIOS_Board_Init(void) { PIOS_DEBUG_Assert(0); } #endif /* PIOS_INCLUDE_GPS */ +#if defined(PIOS_INCLUDE_SPEKTRUM) + /* SPEKTRUM init must come before comms */ + PIOS_SPEKTRUM_Init(); + + if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_COM_Init(&pios_com_spektrum_id, &pios_usart_com_driver, pios_usart_spektrum_id)) { + PIOS_DEBUG_Assert(0); + } + for (uint8_t i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) { + if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], + &pios_spektrum_rcvr_driver, + i)) { + pios_rcvr_max_channel++; + } else { + PIOS_DEBUG_Assert(0); + } + } +#endif #if defined(PIOS_INCLUDE_SBUS) PIOS_SBUS_Init(); @@ -947,6 +962,16 @@ void PIOS_Board_Init(void) { if (PIOS_COM_Init(&pios_com_sbus_id, &pios_usart_com_driver, pios_usart_sbus_id)) { PIOS_DEBUG_Assert(0); } + for (uint8_t i = 0; i < SBUS_NUMBER_OF_CHANNELS; i++) { + if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], + &pios_sbus_rcvr_driver, + i)) { + pios_rcvr_max_channel++; + } else { + PIOS_DEBUG_Assert(0); + } + } + #endif /* PIOS_INCLUDE_SBUS */ #endif /* PIOS_INCLUDE_COM */ @@ -958,10 +983,34 @@ void PIOS_Board_Init(void) { PIOS_GPIO_Init(); #if defined(PIOS_INCLUDE_PWM) +#if (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS) +#error More receiver inputs than available devices +#endif PIOS_PWM_Init(); + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], + &pios_pwm_rcvr_driver, + i)) { + pios_rcvr_max_channel++; + } else { + PIOS_DEBUG_Assert(0); + } + } #endif #if defined(PIOS_INCLUDE_PPM) +#if (PIOS_PPM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS) +#error More receiver inputs than available devices +#endif PIOS_PPM_Init(); + for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS; i++) { + if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], + &pios_ppm_rcvr_driver, + i)) { + pios_rcvr_max_channel++; + } else { + PIOS_DEBUG_Assert(0); + } + } #endif #if defined(PIOS_INCLUDE_USB_HID) PIOS_USB_HID_Init(0); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 0a673f6da..ebeb700ed 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -1,630 +1,623 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotModules OpenPilot Modules - * @{ - * @addtogroup ManualControlModule Manual Control Module - * @brief Provide manual control or allow it alter flight mode. - * @{ - * - * Reads in the ManualControlCommand FlightMode setting from receiver then either - * pass the settings straght to ActuatorDesired object (manual mode) or to - * AttitudeDesired object (stabilized mode) - * - * @file manualcontrol.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief ManualControl module. Handles safety R/C link and flight mode. - * - * @see The GNU Public License (GPL) Version 3 - * - *****************************************************************************/ -/* - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY - * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License - * for more details. - * - * You should have received a copy of the GNU General Public License along - * with this program; if not, write to the Free Software Foundation, Inc., - * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - */ - -#include "openpilot.h" -#include "manualcontrol.h" -#include "manualcontrolsettings.h" -#include "stabilizationsettings.h" -#include "manualcontrolcommand.h" -#include "actuatordesired.h" -#include "stabilizationdesired.h" -#include "flighttelemetrystats.h" -#include "flightstatus.h" -#include "accessorydesired.h" - -// Private constants -#if defined(PIOS_MANUAL_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE -#else -#define STACK_SIZE_BYTES 824 -#endif - -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) -#define UPDATE_PERIOD_MS 20 -#define THROTTLE_FAILSAFE -0.1 -#define FLIGHT_MODE_LIMIT 1.0/3.0 -#define ARMED_TIME_MS 1000 -#define ARMED_THRESHOLD 0.50 -//safe band to allow a bit of calibration error or trim offset (in microseconds) -#define CONNECTION_OFFSET 150 - -// Private types -typedef enum -{ - ARM_STATE_DISARMED, - ARM_STATE_ARMING_MANUAL, - ARM_STATE_ARMED, - ARM_STATE_DISARMING_MANUAL, - ARM_STATE_DISARMING_TIMEOUT -} ArmState_t; - -// Private variables -static xTaskHandle taskHandle; -static ArmState_t armState; -static portTickType lastSysTime; - -// Private functions -static void updateActuatorDesired(ManualControlCommandData * cmd); -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); -static void processFlightMode(ManualControlSettingsData * settings, float flightMode); -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); - -static void manualControlTask(void *parameters); -static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); -static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); -static bool okToArm(void); -static bool validInputRange(int16_t min, int16_t max, uint16_t value); - -#define assumptions1 ( \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ - ) - -#define assumptions3 ( \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ - ) - -#define assumptions5 ( \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ - ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ - ) - - - -#define ARMING_CHANNEL_ROLL 0 -#define ARMING_CHANNEL_PITCH 1 -#define ARMING_CHANNEL_YAW 2 - -#define assumptions7 ( \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \ - ) - -#define assumptions8 ( \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ - ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \ - ) - - -#define assumptions_flightmode ( \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ - ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \ - ) - -#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) - -/** - * Module initialization - */ -int32_t ManualControlInitialize() -{ - /* Check the assumptions about uavobject enum's are correct */ - if(!assumptions) - return -1; - // Start main task - xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); - TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); - PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); - - - return 0; -} - -/** - * Module task - */ -static void manualControlTask(void *parameters) -{ - ManualControlSettingsData settings; - ManualControlCommandData cmd; - FlightStatusData flightStatus; - float flightMode = 0; - - uint8_t disconnected_count = 0; - uint8_t connected_count = 0; - - // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically - // this includes not even registering it if not used - AccessoryDesiredCreateInstance(); - AccessoryDesiredCreateInstance(); - - // Make sure unarmed on power up - ManualControlCommandGet(&cmd); - FlightStatusGet(&flightStatus); - flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; - armState = ARM_STATE_DISARMED; - - // Main task loop - lastSysTime = xTaskGetTickCount(); - while (1) { - float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; - - // Wait until next update - vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); - PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); - - // Read settings - ManualControlSettingsGet(&settings); - - if (ManualControlCommandReadOnly(&cmd)) { - FlightTelemetryStatsData flightTelemStats; - FlightTelemetryStatsGet(&flightTelemStats); - if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { - /* trying to fly via GCS and lost connection. fall back to transmitter */ - UAVObjMetadata metadata; - UAVObjGetMetadata(&cmd, &metadata); - metadata.access = ACCESS_READWRITE; - UAVObjSetMetadata(&cmd, &metadata); - } - } - - if (!ManualControlCommandReadOnly(&cmd)) { - - // Read channel values in us - // TODO: settings.InputMode is currently ignored because PIOS will not allow runtime - // selection of PWM and PPM. The configuration is currently done at compile time in - // the pios_config.h file. - for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { -#if defined(PIOS_INCLUDE_PWM) - cmd.Channel[n] = PIOS_PWM_Get(n); -#elif defined(PIOS_INCLUDE_PPM) - cmd.Channel[n] = PIOS_PPM_Get(n); -#elif defined(PIOS_INCLUDE_SPEKTRUM) - cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); -#elif defined(PIOS_INCLUDE_SBUS) - cmd.Channel[n] = PIOS_SBUS_Get(n); -#endif - scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); - } - - // Check settings, if error raise alarm - if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || - settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || - settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || - settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || - settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - ManualControlCommandSet(&cmd); - continue; - } - - // decide if we have valid manual input or not - bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) && - validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) && - validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) && - validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]); - - // Implement hysteresis loop on connection status - if (valid_input_detected && (++connected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; - connected_count = 0; - disconnected_count = 0; - } else if (!valid_input_detected && (++disconnected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - connected_count = 0; - disconnected_count = 0; - } - - if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - cmd.Throttle = -1; // Shut down engine with no control - cmd.Roll = 0; - cmd.Yaw = 0; - cmd.Pitch = 0; - //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning - // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, - // or leave throttle at IDLE speed or above when going into AUTO-failsafe. - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - ManualControlCommandSet(&cmd); - } else { - AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); - - // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[settings.Roll]; - cmd.Pitch = scaledChannel[settings.Pitch]; - cmd.Yaw = scaledChannel[settings.Yaw]; - cmd.Throttle = scaledChannel[settings.Throttle]; - flightMode = scaledChannel[settings.FlightMode]; - - AccessoryDesiredData accessory; - // Set Accessory 0 - if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory0]; - if(AccessoryDesiredInstSet(0, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - // Set Accessory 1 - if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory1]; - if(AccessoryDesiredInstSet(1, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - // Set Accsesory 2 - if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory2]; - if(AccessoryDesiredInstSet(2, &accessory) != 0) - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); - } - - - processFlightMode(&settings, flightMode); - processArm(&cmd, &settings); - - // Update cmd object - ManualControlCommandSet(&cmd); - - } - - } else { - ManualControlCommandGet(&cmd); /* Under GCS control */ - } - - - FlightStatusGet(&flightStatus); - - // Depending on the mode update the Stabilization or Actuator objects - switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { - case FLIGHTMODE_UNDEFINED: - // This reflects a bug in the code architecture! - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - break; - case FLIGHTMODE_MANUAL: - updateActuatorDesired(&cmd); - break; - case FLIGHTMODE_STABILIZED: - updateStabilizationDesired(&cmd, &settings); - break; - case FLIGHTMODE_GUIDANCE: - // TODO: Implement - break; - } - } -} - -static void updateActuatorDesired(ManualControlCommandData * cmd) -{ - ActuatorDesiredData actuator; - ActuatorDesiredGet(&actuator); - actuator.Roll = cmd->Roll; - actuator.Pitch = cmd->Pitch; - actuator.Yaw = cmd->Yaw; - actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; - ActuatorDesiredSet(&actuator); -} - -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) -{ - StabilizationDesiredData stabilization; - StabilizationDesiredGet(&stabilization); - - StabilizationSettingsData stabSettings; - StabilizationSettingsGet(&stabSettings); - - uint8_t * stab_settings; - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - switch(flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = settings->Stabilization1Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = settings->Stabilization2Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = settings->Stabilization3Settings; - break; - default: - // Major error, this should not occur because only enter this block when one of these is true - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - return; - } - - // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; - stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; - - stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : - 0; // this is an invalid mode - ; - stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - 0; // this is an invalid mode - - stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) : - 0; // this is an invalid mode - - stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; - StabilizationDesiredSet(&stabilization); -} - -/** - * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. - */ -static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) -{ - float valueScaled; - - // Scale - if ((max > min && value >= neutral) || (min > max && value <= neutral)) - { - if (max != neutral) - valueScaled = (float)(value - neutral) / (float)(max - neutral); - else - valueScaled = 0; - } - else - { - if (min != neutral) - valueScaled = (float)(value - neutral) / (float)(neutral - min); - else - valueScaled = 0; - } - - // Bound - if (valueScaled > 1.0) valueScaled = 1.0; - else - if (valueScaled < -1.0) valueScaled = -1.0; - - return valueScaled; -} - -static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { - if(end_time > start_time) - return (end_time - start_time) * portTICK_RATE_MS; - return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; -} - -/** - * @brief Determine if the aircraft is safe to arm - * @returns True if safe to arm, false otherwise - */ -static bool okToArm(void) -{ - // read alarms - SystemAlarmsData alarms; - SystemAlarmsGet(&alarms); - - - // Check each alarm - for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) - { - if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) - { // found an alarm thats set - if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) - continue; - - return false; - } - } - - return true; -} - -/** - * @brief Update the flightStatus object only if value changed. Reduces callbacks - * @param[in] val The new value - */ -static void setArmedIfChanged(uint8_t val) { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - if(flightStatus.Armed != val) { - flightStatus.Armed = val; - FlightStatusSet(&flightStatus); - } -} - -/** - * @brief Process the inputs and determine whether to arm or not - * @param[out] cmd The structure to set the armed in - * @param[in] settings Settings indicating the necessary position - */ -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) -{ - - bool lowThrottle = cmd->Throttle <= 0; - - if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { - // In this configuration we always disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - } else { - // Not really needed since this function not called when disconnected - if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) - return; - - // The throttle is not low, in case we where arming or disarming, abort - if (!lowThrottle) { - switch(armState) { - case ARM_STATE_DISARMING_MANUAL: - case ARM_STATE_DISARMING_TIMEOUT: - armState = ARM_STATE_ARMED; - break; - case ARM_STATE_ARMING_MANUAL: - armState = ARM_STATE_DISARMED; - break; - default: - // Nothing needs to be done in the other states - break; - } - return; - } - - // The rest of these cases throttle is low - if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { - // In this configuration, we go into armed state as soon as the throttle is low, never disarm - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - return; - } - - - // When the configuration is not "Always armed" and no "Always disarmed", - // the state will not be changed when the throttle is not low - static portTickType armedDisarmStart; - float armingInputLevel = 0; - - // Calc channel see assumptions7 - int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; - switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { - case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break; - case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; - case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; - } - - bool manualArm = false; - bool manualDisarm = false; - - if (armingInputLevel <= -ARMED_THRESHOLD) - manualArm = true; - else if (armingInputLevel >= +ARMED_THRESHOLD) - manualDisarm = true; - - switch(armState) { - case ARM_STATE_DISARMED: - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - - // only allow arming if it's OK too - if (manualArm && okToArm()) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_ARMING_MANUAL; - } - break; - - case ARM_STATE_ARMING_MANUAL: - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); - - if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_ARMED; - else if (!manualArm) - armState = ARM_STATE_DISARMED; - break; - - case ARM_STATE_ARMED: - // When we get here, the throttle is low, - // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_TIMEOUT; - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - break; - - case ARM_STATE_DISARMING_TIMEOUT: - // We get here when armed while throttle low, even when the arming timeout is not enabled - if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) - armState = ARM_STATE_DISARMED; - - // Switch to disarming due to manual control when needed - if (manualDisarm) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_MANUAL; - } - break; - - case ARM_STATE_DISARMING_MANUAL: - if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) - armState = ARM_STATE_DISARMED; - else if (!manualDisarm) - armState = ARM_STATE_ARMED; - break; - } // End Switch - } -} - -/** - * @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly - * @param[out] cmd Pointer to the command structure to set the flight mode in - * @param[in] settings The settings which indicate which position is which mode - * @param[in] flightMode the value of the switch position - */ -static void processFlightMode(ManualControlSettingsData * settings, float flightMode) -{ - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - - uint8_t newMode; - // Note here the code is ass - if (flightMode < -FLIGHT_MODE_LIMIT) - newMode = settings->FlightModePosition[0]; - else if (flightMode > FLIGHT_MODE_LIMIT) - newMode = settings->FlightModePosition[2]; - else - newMode = settings->FlightModePosition[1]; - - if(flightStatus.FlightMode != newMode) { - flightStatus.FlightMode = newMode; - FlightStatusSet(&flightStatus); - } - -} - -/** - * @brief Determine if the manual input value is within acceptable limits - * @returns return TRUE if so, otherwise return FALSE - */ -bool validInputRange(int16_t min, int16_t max, uint16_t value) -{ - if (min > max) - { - int16_t tmp = min; - min = max; - max = tmp; - } - return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); -} - -/** - * @} - * @} - */ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ManualControlModule Manual Control Module + * @brief Provide manual control or allow it alter flight mode. + * @{ + * + * Reads in the ManualControlCommand FlightMode setting from receiver then either + * pass the settings straght to ActuatorDesired object (manual mode) or to + * AttitudeDesired object (stabilized mode) + * + * @file manualcontrol.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief ManualControl module. Handles safety R/C link and flight mode. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include "openpilot.h" +#include "manualcontrol.h" +#include "manualcontrolsettings.h" +#include "stabilizationsettings.h" +#include "manualcontrolcommand.h" +#include "actuatordesired.h" +#include "stabilizationdesired.h" +#include "flighttelemetrystats.h" +#include "flightstatus.h" +#include "accessorydesired.h" + +// Private constants +#if defined(PIOS_MANUAL_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE +#else +#define STACK_SIZE_BYTES 824 +#endif + +#define TASK_PRIORITY (tskIDLE_PRIORITY+4) +#define UPDATE_PERIOD_MS 20 +#define THROTTLE_FAILSAFE -0.1 +#define FLIGHT_MODE_LIMIT 1.0/3.0 +#define ARMED_TIME_MS 1000 +#define ARMED_THRESHOLD 0.50 +//safe band to allow a bit of calibration error or trim offset (in microseconds) +#define CONNECTION_OFFSET 150 + +// Private types +typedef enum +{ + ARM_STATE_DISARMED, + ARM_STATE_ARMING_MANUAL, + ARM_STATE_ARMED, + ARM_STATE_DISARMING_MANUAL, + ARM_STATE_DISARMING_TIMEOUT +} ArmState_t; + +// Private variables +static xTaskHandle taskHandle; +static ArmState_t armState; +static portTickType lastSysTime; + +// Private functions +static void updateActuatorDesired(ManualControlCommandData * cmd); +static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); +static void processFlightMode(ManualControlSettingsData * settings, float flightMode); +static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); + +static void manualControlTask(void *parameters); +static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral); +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); +static bool okToArm(void); +static bool validInputRange(int16_t min, int16_t max, uint16_t value); + +#define assumptions1 ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION1SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) + +#define assumptions3 ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION2SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) + +#define assumptions5 ( \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_NONE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_RATE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) && \ + ((int)MANUALCONTROLSETTINGS_STABILIZATION3SETTINGS_ATTITUDE == (int)STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) \ + ) + + + +#define ARMING_CHANNEL_ROLL 0 +#define ARMING_CHANNEL_PITCH 1 +#define ARMING_CHANNEL_YAW 2 + +#define assumptions7 ( \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_ROLL) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_PITCH) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 == ARMING_CHANNEL_YAW) \ + ) + +#define assumptions8 ( \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_ROLLRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHFORWARD -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_PITCHAFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWLEFT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 == 0) && \ + ( ((int)MANUALCONTROLSETTINGS_ARMING_YAWRIGHT -(int)MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2 != 0) \ + ) + + +#define assumptions_flightmode ( \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_MANUAL == (int) FLIGHTSTATUS_FLIGHTMODE_MANUAL) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED1 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED1) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED2 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED2) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_STABILIZED3 == (int) FLIGHTSTATUS_FLIGHTMODE_STABILIZED3) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_VELOCITYCONTROL == (int) FLIGHTSTATUS_FLIGHTMODE_VELOCITYCONTROL) && \ + ( (int)MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD == (int) FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) \ + ) + +#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) + +/** + * Module initialization + */ +int32_t ManualControlInitialize() +{ + /* Check the assumptions about uavobject enum's are correct */ + if(!assumptions) + return -1; + // Start main task + xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle); + TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); + PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); + + + return 0; +} + +/** + * Module task + */ +static void manualControlTask(void *parameters) +{ + ManualControlSettingsData settings; + ManualControlCommandData cmd; + FlightStatusData flightStatus; + float flightMode = 0; + + uint8_t disconnected_count = 0; + uint8_t connected_count = 0; + + // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically + // this includes not even registering it if not used + AccessoryDesiredCreateInstance(); + AccessoryDesiredCreateInstance(); + + // Make sure unarmed on power up + ManualControlCommandGet(&cmd); + FlightStatusGet(&flightStatus); + flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; + armState = ARM_STATE_DISARMED; + + // Main task loop + lastSysTime = xTaskGetTickCount(); + while (1) { + float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; + + // Wait until next update + vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); + PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); + + // Read settings + ManualControlSettingsGet(&settings); + + if (ManualControlCommandReadOnly(&cmd)) { + FlightTelemetryStatsData flightTelemStats; + FlightTelemetryStatsGet(&flightTelemStats); + if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + /* trying to fly via GCS and lost connection. fall back to transmitter */ + UAVObjMetadata metadata; + UAVObjGetMetadata(&cmd, &metadata); + metadata.access = ACCESS_READWRITE; + UAVObjSetMetadata(&cmd, &metadata); + } + } + + if (!ManualControlCommandReadOnly(&cmd)) { + + // Read channel values in us + for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { + if (pios_rcvr_channel_to_id_map[n]) { + cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_channel_to_id_map[n]); + } else { + cmd.Channel[n] = -1; + } + scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); + } + + // Check settings, if error raise alarm + if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || + settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || + settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || + settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || + settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + ManualControlCommandSet(&cmd); + continue; + } + + // decide if we have valid manual input or not + bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) && + validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) && + validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) && + validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]); + + // Implement hysteresis loop on connection status + if (valid_input_detected && (++connected_count > 10)) { + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; + connected_count = 0; + disconnected_count = 0; + } else if (!valid_input_detected && (++disconnected_count > 10)) { + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + connected_count = 0; + disconnected_count = 0; + } + + if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { + cmd.Throttle = -1; // Shut down engine with no control + cmd.Roll = 0; + cmd.Yaw = 0; + cmd.Pitch = 0; + //cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning + // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, + // or leave throttle at IDLE speed or above when going into AUTO-failsafe. + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + ManualControlCommandSet(&cmd); + } else { + AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); + + // Scale channels to -1 -> +1 range + cmd.Roll = scaledChannel[settings.Roll]; + cmd.Pitch = scaledChannel[settings.Pitch]; + cmd.Yaw = scaledChannel[settings.Yaw]; + cmd.Throttle = scaledChannel[settings.Throttle]; + flightMode = scaledChannel[settings.FlightMode]; + + AccessoryDesiredData accessory; + // Set Accessory 0 + if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) { + accessory.AccessoryVal = scaledChannel[settings.Accessory0]; + if(AccessoryDesiredInstSet(0, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accessory 1 + if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) { + accessory.AccessoryVal = scaledChannel[settings.Accessory1]; + if(AccessoryDesiredInstSet(1, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + // Set Accsesory 2 + if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) { + accessory.AccessoryVal = scaledChannel[settings.Accessory2]; + if(AccessoryDesiredInstSet(2, &accessory) != 0) + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + + + processFlightMode(&settings, flightMode); + processArm(&cmd, &settings); + + // Update cmd object + ManualControlCommandSet(&cmd); + + } + + } else { + ManualControlCommandGet(&cmd); /* Under GCS control */ + } + + + FlightStatusGet(&flightStatus); + + // Depending on the mode update the Stabilization or Actuator objects + switch(PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { + case FLIGHTMODE_UNDEFINED: + // This reflects a bug in the code architecture! + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + break; + case FLIGHTMODE_MANUAL: + updateActuatorDesired(&cmd); + break; + case FLIGHTMODE_STABILIZED: + updateStabilizationDesired(&cmd, &settings); + break; + case FLIGHTMODE_GUIDANCE: + // TODO: Implement + break; + } + } +} + +static void updateActuatorDesired(ManualControlCommandData * cmd) +{ + ActuatorDesiredData actuator; + ActuatorDesiredGet(&actuator); + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; + actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + ActuatorDesiredSet(&actuator); +} + +static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +{ + StabilizationDesiredData stabilization; + StabilizationDesiredGet(&stabilization); + + StabilizationSettingsData stabSettings; + StabilizationSettingsGet(&stabSettings); + + uint8_t * stab_settings; + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch(flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + stab_settings = settings->Stabilization1Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + stab_settings = settings->Stabilization2Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + stab_settings = settings->Stabilization3Settings; + break; + default: + // Major error, this should not occur because only enter this block when one of these is true + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + return; + } + + // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; + + stabilization.Roll = (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + 0; // this is an invalid mode + ; + stabilization.Pitch = (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + 0; // this is an invalid mode + + stabilization.Yaw = (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? fmod(cmd->Yaw * 180.0, 360) : + 0; // this is an invalid mode + + stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; + StabilizationDesiredSet(&stabilization); +} + +/** + * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. + */ +static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) +{ + float valueScaled; + + // Scale + if ((max > min && value >= neutral) || (min > max && value <= neutral)) + { + if (max != neutral) + valueScaled = (float)(value - neutral) / (float)(max - neutral); + else + valueScaled = 0; + } + else + { + if (min != neutral) + valueScaled = (float)(value - neutral) / (float)(neutral - min); + else + valueScaled = 0; + } + + // Bound + if (valueScaled > 1.0) valueScaled = 1.0; + else + if (valueScaled < -1.0) valueScaled = -1.0; + + return valueScaled; +} + +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) { + if(end_time > start_time) + return (end_time - start_time) * portTICK_RATE_MS; + return ((((portTICK_RATE_MS) -1) - start_time) + end_time) * portTICK_RATE_MS; +} + +/** + * @brief Determine if the aircraft is safe to arm + * @returns True if safe to arm, false otherwise + */ +static bool okToArm(void) +{ + // read alarms + SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); + + + // Check each alarm + for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) + { + if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) + { // found an alarm thats set + if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) + continue; + + return false; + } + } + + return true; +} + +/** + * @brief Update the flightStatus object only if value changed. Reduces callbacks + * @param[in] val The new value + */ +static void setArmedIfChanged(uint8_t val) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + if(flightStatus.Armed != val) { + flightStatus.Armed = val; + FlightStatusSet(&flightStatus); + } +} + +/** + * @brief Process the inputs and determine whether to arm or not + * @param[out] cmd The structure to set the armed in + * @param[in] settings Settings indicating the necessary position + */ +static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +{ + + bool lowThrottle = cmd->Throttle <= 0; + + if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) { + // In this configuration we always disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + } else { + // Not really needed since this function not called when disconnected + if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) + return; + + // The throttle is not low, in case we where arming or disarming, abort + if (!lowThrottle) { + switch(armState) { + case ARM_STATE_DISARMING_MANUAL: + case ARM_STATE_DISARMING_TIMEOUT: + armState = ARM_STATE_ARMED; + break; + case ARM_STATE_ARMING_MANUAL: + armState = ARM_STATE_DISARMED; + break; + default: + // Nothing needs to be done in the other states + break; + } + return; + } + + // The rest of these cases throttle is low + if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) { + // In this configuration, we go into armed state as soon as the throttle is low, never disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + return; + } + + + // When the configuration is not "Always armed" and no "Always disarmed", + // the state will not be changed when the throttle is not low + static portTickType armedDisarmStart; + float armingInputLevel = 0; + + // Calc channel see assumptions7 + int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1; + switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) { + case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break; + case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break; + case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break; + } + + bool manualArm = false; + bool manualDisarm = false; + + if (armingInputLevel <= -ARMED_THRESHOLD) + manualArm = true; + else if (armingInputLevel >= +ARMED_THRESHOLD) + manualDisarm = true; + + switch(armState) { + case ARM_STATE_DISARMED: + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + + // only allow arming if it's OK too + if (manualArm && okToArm()) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_ARMING_MANUAL; + } + break; + + case ARM_STATE_ARMING_MANUAL: + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); + + if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) + armState = ARM_STATE_ARMED; + else if (!manualArm) + armState = ARM_STATE_DISARMED; + break; + + case ARM_STATE_ARMED: + // When we get here, the throttle is low, + // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_TIMEOUT; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + break; + + case ARM_STATE_DISARMING_TIMEOUT: + // We get here when armed while throttle low, even when the arming timeout is not enabled + if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout)) + armState = ARM_STATE_DISARMED; + + // Switch to disarming due to manual control when needed + if (manualDisarm) { + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_MANUAL; + } + break; + + case ARM_STATE_DISARMING_MANUAL: + if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) + armState = ARM_STATE_DISARMED; + else if (!manualDisarm) + armState = ARM_STATE_ARMED; + break; + } // End Switch + } +} + +/** + * @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly + * @param[out] cmd Pointer to the command structure to set the flight mode in + * @param[in] settings The settings which indicate which position is which mode + * @param[in] flightMode the value of the switch position + */ +static void processFlightMode(ManualControlSettingsData * settings, float flightMode) +{ + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + + uint8_t newMode; + // Note here the code is ass + if (flightMode < -FLIGHT_MODE_LIMIT) + newMode = settings->FlightModePosition[0]; + else if (flightMode > FLIGHT_MODE_LIMIT) + newMode = settings->FlightModePosition[2]; + else + newMode = settings->FlightModePosition[1]; + + if(flightStatus.FlightMode != newMode) { + flightStatus.FlightMode = newMode; + FlightStatusSet(&flightStatus); + } + +} + +/** + * @brief Determine if the manual input value is within acceptable limits + * @returns return TRUE if so, otherwise return FALSE + */ +bool validInputRange(int16_t min, int16_t max, uint16_t value) +{ + if (min > max) + { + int16_t tmp = min; + min = max; + max = tmp; + } + return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); +} + +/** + * @} + * @} + */ diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index 68ad6f561..83b179641 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -196,6 +196,7 @@ SRC += $(PIOSCOMMON)/pios_hcsr04.c SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_iap.c SRC += $(PIOSCOMMON)/pios_bl_helper.c +SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/printf-stdarg.c SRC += $(FLIGHTLIB)/ahrs_spi_comm.c SRC += $(FLIGHTLIB)/ahrs_comm_objects.c diff --git a/flight/OpenPilot/System/inc/pios_config.h b/flight/OpenPilot/System/inc/pios_config.h index 939ee73b6..215d0fadb 100644 --- a/flight/OpenPilot/System/inc/pios_config.h +++ b/flight/OpenPilot/System/inc/pios_config.h @@ -42,6 +42,8 @@ #define PIOS_INCLUDE_IRQ #define PIOS_INCLUDE_LED +#define PIOS_INCLUDE_RCVR + #if defined(USE_SPEKTRUM) #define PIOS_INCLUDE_SPEKTRUM #elif defined(USE_SBUS) diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 52a059777..7c9e13d82 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -497,13 +497,13 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = { }, }; +#include static uint32_t pios_usart_spektrum_id; void PIOS_USART_spektrum_irq_handler(void) { - SPEKTRUM_IRQHandler(pios_usart_spektrum_id); + PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id); } -#include void RTC_IRQHandler(); void RTC_IRQHandler() __attribute__ ((alias ("PIOS_SUPV_irq_handler"))); const struct pios_spektrum_cfg pios_spektrum_cfg = { @@ -525,11 +525,11 @@ const struct pios_spektrum_cfg pios_spektrum_cfg = { .pin = GPIO_Pin_10, }; -void PIOS_SUPV_irq_handler() { +void PIOS_SUPV_irq_handler(void) { if (RTC_GetITStatus(RTC_IT_SEC)) { /* Call the right handler */ - PIOS_SPEKTRUM_irq_handler(pios_usart_spektrum_id); + PIOS_SPEKTRUMSV_irq_handler(pios_usart_spektrum_id); /* Wait until last write operation on RTC registers has finished */ RTC_WaitForLastTask(); @@ -802,7 +802,7 @@ const struct pios_ppmsv_cfg pios_ppmsv_cfg = { .ccr = TIM_IT_Update, }; -void PIOS_TIM6_irq_handler() +void PIOS_TIM6_irq_handler(void) { PIOS_PPMSV_irq_handler(); } @@ -844,7 +844,7 @@ const struct pios_ppm_cfg pios_ppm_cfg = { .ccr = TIM_IT_CC2, }; -void PIOS_TIM1_CC_irq_handler() +void PIOS_TIM1_CC_irq_handler(void) { PIOS_PPM_irq_handler(); } @@ -1013,6 +1013,13 @@ static const struct stm32_gpio pios_debug_pins[] = { #endif /* PIOS_ENABLE_DEBUG_PINS */ +#if defined(PIOS_INCLUDE_RCVR) +#include "pios_rcvr_priv.h" + +uint32_t pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_DEVS]; +uint32_t pios_rcvr_max_channel; +#endif /* PIOS_INCLUDE_RCVR */ + extern const struct pios_com_driver pios_usb_com_driver; uint32_t pios_com_telem_rf_id; @@ -1050,18 +1057,6 @@ void PIOS_Board_Init(void) { PIOS_SDCARD_MountFS(0); #endif /* PIOS_INCLUDE_SPI */ -#if defined(PIOS_INCLUDE_SPEKTRUM) - /* SPEKTRUM init must come before comms */ - PIOS_RTC_Init(); // Spektrum uses RTC to check for frame failures - PIOS_SPEKTRUM_Init(); - - if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { - PIOS_DEBUG_Assert(0); - } - if (PIOS_COM_Init(&pios_com_spektrum_id, &pios_usart_com_driver, pios_usart_spektrum_id)) { - PIOS_DEBUG_Assert(0); - } -#endif /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -1105,11 +1100,59 @@ void PIOS_Board_Init(void) { PIOS_ADC_Init(); PIOS_GPIO_Init(); +#if defined(PIOS_INCLUDE_SPEKTRUM) +#if (PIOS_SPEKTRUM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS) +#error More receiver inputs than available devices +#endif + /* SPEKTRUM init must come before comms */ + PIOS_SPEKTRUM_Init(); + + if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { + PIOS_DEBUG_Assert(0); + } + if (PIOS_COM_Init(&pios_com_spektrum_id, &pios_usart_com_driver, pios_usart_spektrum_id)) { + PIOS_DEBUG_Assert(0); + } + for (uint8_t i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) { + if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], + &pios_spektrum_rcvr_driver, + i)) { + pios_rcvr_max_channel++; + } else { + PIOS_DEBUG_Assert(0); + } + } +#endif + #if defined(PIOS_INCLUDE_PWM) +#if (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS) +#error More receiver inputs than available devices +#endif PIOS_PWM_Init(); + for (uint8_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { + if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], + &pios_pwm_rcvr_driver, + i)) { + pios_rcvr_max_channel++; + } else { + PIOS_DEBUG_Assert(0); + } + } #endif #if defined(PIOS_INCLUDE_PPM) +#if (PIOS_PPM_NUM_INPUTS > PIOS_RCVR_MAX_DEVS) +#error More receiver inputs than available devices +#endif PIOS_PPM_Init(); + for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS; i++) { + if (!PIOS_RCVR_Init(&pios_rcvr_channel_to_id_map[pios_rcvr_max_channel], + &pios_ppm_rcvr_driver, + i)) { + pios_rcvr_max_channel++; + } else { + PIOS_DEBUG_Assert(0); + } + } #endif #if defined(PIOS_INCLUDE_USB_HID) PIOS_USB_HID_Init(0); diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index b5b570e53..993d08c78 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -225,10 +225,27 @@ extern uint32_t pios_com_sbus_id; #define PIOS_ADC_RATE (72.0e6 / 1.0 / 8.0 / 252.0 / (PIOS_ADC_NUM_CHANNELS >> PIOS_ADC_USE_ADC2)) #define PIOS_ADC_MAX_OVERSAMPLING 36 +//------------------------ +// PIOS_RCVR +// See also pios_board.c +//------------------------ +#define PIOS_RCVR_MAX_DEVS 12 + //------------------------- -// Receiver PWM inputs +// Receiver PPM input //------------------------- -#define PIOS_PWM_MAX_INPUTS 6 +#define PIOS_PPM_NUM_INPUTS 6 //Could be more if needed +#define PIOS_PPM_SUPV_ENABLED 1 + +//------------------------- +// Receiver PWM input +//------------------------- +#define PIOS_PWM_NUM_INPUTS 6 + +//------------------------- +// Receiver SPEKTRUM input +//------------------------- +#define PIOS_SPEKTRUM_NUM_INPUTS 12 //------------------------- // Servo outputs diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h index e82858fb1..dc598cf4b 100644 --- a/flight/PiOS/Boards/STM3210E_OP.h +++ b/flight/PiOS/Boards/STM3210E_OP.h @@ -199,37 +199,33 @@ extern uint32_t pios_com_sbus_id; #define PIOS_IRQ_PRIO_HIGH 5 // for SPI, ADC, I2C etc... #define PIOS_IRQ_PRIO_HIGHEST 4 // for USART etc... -//------------------------- -// Receiver PWM inputs -//------------------------- -/*#define PIOS_PWM_SUPV_ENABLED 1 -#define PIOS_PWM_SUPV_TIMER TIM6 -#define PIOS_PWM_SUPV_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE) -#define PIOS_PWM_SUPV_HZ 25 -#define PIOS_PWM_SUPV_IRQ_CHANNEL TIM6_IRQn -#define PIOS_PWM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void)*/ +//------------------------ +// PIOS_RCVR +// See also pios_board.c +//------------------------ +#define PIOS_RCVR_MAX_DEVS 12 //------------------------- // Receiver PPM input //------------------------- #define PIOS_PPM_NUM_INPUTS 8 //Could be more if needed #define PIOS_PPM_SUPV_ENABLED 1 + //------------------------- -// SPEKTRUM input +// Receiver PWM input //------------------------- -//#define PIOS_SPEKTRUM_SUPV_ENABLED 1 -//#define PIOS_SPEKTRUM_SUPV_TIMER TIM6 -//#define PIOS_SPEKTRUM_SUPV_TIMER_RCC_FUNC RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE) -//#define PIOS_SPEKTRUM_SUPV_HZ 60 // 1/22ms -//#define PIOS_SPEKTRUM_SUPV_IRQ_CHANNEL TIM6_IRQn -//#define PIOS_SPEKTRUM_SUPV_IRQ_FUNC void TIM6_IRQHandler(void) +#define PIOS_PWM_NUM_INPUTS 8 + +//------------------------- +// Receiver SPEKTRUM input +//------------------------- +#define PIOS_SPEKTRUM_NUM_INPUTS 12 //------------------------- // Servo outputs //------------------------- #define PIOS_SERVO_UPDATE_HZ 50 #define PIOS_SERVOS_INITIAL_POSITION 0 /* dont want to start motors, have no pulse till settings loaded */ -#define PIOS_PWM_MAX_INPUTS 8 //------------------------- // ADC diff --git a/flight/PiOS/STM32F10x/pios_ppm.c b/flight/PiOS/STM32F10x/pios_ppm.c index 885b3e27d..b9f087812 100644 --- a/flight/PiOS/STM32F10x/pios_ppm.c +++ b/flight/PiOS/STM32F10x/pios_ppm.c @@ -34,8 +34,14 @@ #if defined(PIOS_INCLUDE_PPM) -/* Local Variables */ +/* Provide a RCVR driver */ +static int32_t PIOS_PPM_Get(uint32_t chan_id); +const struct pios_rcvr_driver pios_ppm_rcvr_driver = { + .read = PIOS_PPM_Get, +}; + +/* Local Variables */ static TIM_ICInitTypeDef TIM_ICInitStructure; static uint8_t PulseIndex; static uint32_t PreviousValue; @@ -208,13 +214,13 @@ void PIOS_PPM_Init(void) * \output -1 Channel not available * \output >0 Channel value */ -int32_t PIOS_PPM_Get(int8_t Channel) +static int32_t PIOS_PPM_Get(uint32_t chan_id) { /* Return error if channel not available */ - if (Channel >= PIOS_PPM_NUM_INPUTS) { + if (chan_id >= PIOS_PPM_NUM_INPUTS) { return -1; } - return CaptureValue[Channel]; + return CaptureValue[chan_id]; } /** diff --git a/flight/PiOS/STM32F10x/pios_pwm.c b/flight/PiOS/STM32F10x/pios_pwm.c index c524b3d9f..e00da7cb9 100644 --- a/flight/PiOS/STM32F10x/pios_pwm.c +++ b/flight/PiOS/STM32F10x/pios_pwm.c @@ -34,15 +34,20 @@ #if defined(PIOS_INCLUDE_PWM) -/* Local Variables */ -static uint8_t CaptureState[PIOS_PWM_MAX_INPUTS]; -static uint16_t RiseValue[PIOS_PWM_MAX_INPUTS]; -static uint16_t FallValue[PIOS_PWM_MAX_INPUTS]; -static uint32_t CaptureValue[PIOS_PWM_MAX_INPUTS]; +/* Provide a RCVR driver */ +static int32_t PIOS_PWM_Get(uint32_t chan_id); -//static uint8_t SupervisorState = 0; -static uint32_t CapCounter[PIOS_PWM_MAX_INPUTS]; -//static uint32_t CapCounterPrev[MAX_CHANNELS]; +const struct pios_rcvr_driver pios_pwm_rcvr_driver = { + .read = PIOS_PWM_Get, +}; + +/* Local Variables */ +static uint8_t CaptureState[PIOS_PWM_NUM_INPUTS]; +static uint16_t RiseValue[PIOS_PWM_NUM_INPUTS]; +static uint16_t FallValue[PIOS_PWM_NUM_INPUTS]; +static uint32_t CaptureValue[PIOS_PWM_NUM_INPUTS]; + +static uint32_t CapCounter[PIOS_PWM_NUM_INPUTS]; /** * Initialises all the pins @@ -127,52 +132,6 @@ void PIOS_PWM_Init(void) /* Warning, I don't think this will work for multiple remaps at once */ GPIO_PinRemapConfig(pios_pwm_cfg.remap, ENABLE); } - -#if 0 - /* Supervisor Setup */ -#if (PIOS_PWM_SUPV_ENABLED) - /* Flush counter variables */ - for (i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - CapCounter[i] = 0; - } - for (i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - CapCounterPrev[i] = 0; - } - - /* Enable timer clock */ - PIOS_PWM_SUPV_TIMER_RCC_FUNC; - - /* Configure interrupts */ - NVIC_InitStructure.NVIC_IRQChannel = PIOS_PWM_SUPV_IRQ_CHANNEL; - NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID; - NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; - NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; - NVIC_Init(&NVIC_InitStructure); - - /* Time base configuration */ - TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); - TIM_TimeBaseStructure.TIM_Period = ((1000000 / PIOS_PWM_SUPV_HZ) - 1); - TIM_TimeBaseStructure.TIM_Prescaler = (PIOS_MASTER_CLOCK / 1000000) - 1; /* For 1 uS accuracy */ - TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; - TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; - TIM_TimeBaseInit(PIOS_PWM_SUPV_TIMER, &TIM_TimeBaseStructure); - - /* Enable the CC2 Interrupt Request */ - TIM_ITConfig(PIOS_PWM_SUPV_TIMER, TIM_IT_Update, ENABLE); - - /* Clear update pending flag */ - TIM_ClearFlag(TIM2, TIM_FLAG_Update); - - /* Enable counter */ - TIM_Cmd(PIOS_PWM_SUPV_TIMER, ENABLE); -#endif - - /* Setup local variable which stays in this scope */ - /* Doing this here and using a local variable saves doing it in the ISR */ - TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; - TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0x0; -#endif } /** @@ -181,13 +140,13 @@ void PIOS_PWM_Init(void) * \output -1 Channel not available * \output >0 Channel value */ -int32_t PIOS_PWM_Get(int8_t Channel) +static int32_t PIOS_PWM_Get(uint32_t chan_id) { /* Return error if channel not available */ - if (Channel >= pios_pwm_cfg.num_channels) { + if (chan_id >= pios_pwm_cfg.num_channels) { return -1; } - return CaptureValue[Channel]; + return CaptureValue[chan_id]; } void PIOS_PWM_irq_handler(TIM_TypeDef * timer) @@ -254,84 +213,6 @@ void PIOS_PWM_irq_handler(TIM_TypeDef * timer) } } -#if 0 -/** -* Handle TIM5 global interrupt request -*/ -void TIM5_IRQHandler(void) -{ - /* Do this as it's more efficient */ - if (TIM_GetITStatus(PIOS_PWM_TIM_PORT[2], PIOS_PWM_TIM_CCR[2]) == SET) { - if (CaptureState[2] == 0) { - RiseValue[2] = TIM_GetCapture1(PIOS_PWM_TIM_PORT[2]); - } else { - FallValue[2] = TIM_GetCapture1(PIOS_PWM_TIM_PORT[2]); - } - - /* Clear TIM3 Capture compare interrupt pending bit */ - TIM_ClearITPendingBit(PIOS_PWM_TIM_PORT[2], PIOS_PWM_TIM_CCR[2]); - - /* Simple rise or fall state machine */ - if (CaptureState[2] == 0) { - /* Switch states */ - CaptureState[2] = 1; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Falling; - TIM_ICInitStructure.TIM_Channel = PIOS_PWM_TIM_CHANNEL[2]; - TIM_ICInit(PIOS_PWM_TIM_PORT[2], &TIM_ICInitStructure); - - } else { - /* Capture computation */ - if (FallValue[2] > RiseValue[2]) { - CaptureValue[2] = (FallValue[2] - RiseValue[2]); - } else { - CaptureValue[2] = ((0xFFFF - RiseValue[2]) + FallValue[2]); - } - - /* Switch states */ - CaptureState[2] = 0; - - /* Increase supervisor counter */ - CapCounter[2]++; - - /* Switch polarity of input capture */ - TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; - TIM_ICInitStructure.TIM_Channel = PIOS_PWM_TIM_CHANNEL[2]; - TIM_ICInit(PIOS_PWM_TIM_PORT[2], &TIM_ICInitStructure); - } - } -} - -/** -* This function handles TIM3 global interrupt request. -*/ -PIOS_PWM_SUPV_IRQ_FUNC { - /* Clear timer interrupt pending bit */ - TIM_ClearITPendingBit(PIOS_PWM_SUPV_TIMER, TIM_IT_Update); - - /* Simple state machine */ - if (SupervisorState == 0) { - /* Save this states values */ - for (int32_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - CapCounterPrev[i] = CapCounter[i]; - } - - /* Move to next state */ - SupervisorState = 1; - } else { - /* See what channels have been updated */ - for (int32_t i = 0; i < PIOS_PWM_NUM_INPUTS; i++) { - if (CapCounter[i] == CapCounterPrev[i]) { - CaptureValue[i] = 0; - } - } - - /* Move to next state */ - SupervisorState = 0; - } -} -#endif #endif /** diff --git a/flight/PiOS/STM32F10x/pios_sbus.c b/flight/PiOS/STM32F10x/pios_sbus.c index c742397a5..823597641 100644 --- a/flight/PiOS/STM32F10x/pios_sbus.c +++ b/flight/PiOS/STM32F10x/pios_sbus.c @@ -36,6 +36,13 @@ /* Global Variables */ +/* Provide a RCVR driver */ +static int32_t PIOS_SBUS_Get(uint32_t chan_id); + +const struct pios_rcvr_driver pios_sbus_rcvr_driver = { + .read = PIOS_SBUS_Get, +}; + /* Local Variables */ static uint16_t channel_data[SBUS_NUMBER_OF_CHANNELS]; static uint8_t received_data[SBUS_FRAME_LENGTH - 2]; @@ -154,19 +161,19 @@ void PIOS_SBUS_Init(void) * \output -1 channel not available * \output >0 channel value */ -int16_t PIOS_SBUS_Get(int8_t channel) +static int32_t PIOS_SBUS_Get(uint32_t chan_id) { /* return error if channel is not available */ - if (channel >= SBUS_NUMBER_OF_CHANNELS) { + if (chan_id >= SBUS_NUMBER_OF_CHANNELS) { return -1; } - return channel_data[channel]; + return channel_data[chan_id]; } /** * Interrupt handler for USART */ -void SBUS_IRQHandler(uint32_t usart_id) +void PIOS_SBUS_irq_handler(uint32_t usart_id) { /* by always reading DR after SR make sure to clear any error interrupts */ volatile uint16_t sr = pios_sbus_cfg.pios_usart_sbus_cfg->regs->SR; @@ -198,7 +205,7 @@ void SBUS_IRQHandler(uint32_t usart_id) * data reception. If no new data received in 100ms, we must call the * failsafe function which clears all channels. */ -void PIOS_SBUS_irq_handler() +void PIOS_SBUSSV_irq_handler() { /* waiting for new frame if no bytes were received in 3.2ms */ if (++receive_timer > 2) { diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 8b00a32a3..6e236991d 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -51,8 +51,15 @@ /* Global Variables */ +/* Provide a RCVR driver */ +static int32_t PIOS_SPEKTRUM_Get(uint32_t chan_id); + +const struct pios_rcvr_driver pios_spektrum_rcvr_driver = { + .read = PIOS_SPEKTRUM_Get, +}; + /* Local Variables */ -static uint16_t CaptureValue[12],CaptureValueTemp[12]; +static uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS],CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS]; static uint8_t prev_byte = 0xFF, sync = 0, bytecount = 0, datalength=0, frame_error=0, byte_array[20] = { 0 }; uint8_t sync_of = 0; uint16_t supv_timer=0; @@ -84,13 +91,13 @@ void PIOS_SPEKTRUM_Init(void) * \output -1 Channel not available * \output >0 Channel value */ -int16_t PIOS_SPEKTRUM_Get(int8_t Channel) +static int32_t PIOS_SPEKTRUM_Get(uint32_t chan_id) { /* Return error if channel not available */ - if (Channel >= 12) { + if (chan_id >= PIOS_SPEKTRUM_NUM_INPUTS) { return -1; } - return CaptureValue[Channel]; + return CaptureValue[chan_id]; } /** @@ -201,7 +208,7 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b) { frame_error=1; } - if (channeln < 12 && !frame_error) + if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !frame_error) CaptureValueTemp[channeln] = data; } } @@ -212,7 +219,7 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b) sync_of = 0; if (!frame_error) { - for(int i=0;i<12;i++) + for(int i=0;iregs->SR; volatile uint8_t b = pios_spektrum_cfg.pios_usart_spektrum_cfg->regs->DR; @@ -248,7 +255,7 @@ void SPEKTRUM_IRQHandler(uint32_t usart_id) { *@brief This function is called between frames and when a spektrum word hasnt been decoded for too long *@brief clears the channel values */ -void PIOS_SPEKTRUM_irq_handler() { +void PIOS_SPEKTRUMSV_irq_handler() { /* 125hz */ supv_timer++; if(supv_timer > 5) { @@ -262,7 +269,7 @@ void PIOS_SPEKTRUM_irq_handler() { if (sync_of > 12) { /* signal lost */ sync_of = 0; - for (int i = 0; i < 12; i++) { + for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) { CaptureValue[i] = 0; CaptureValueTemp[i] = 0; } diff --git a/flight/PiOS/inc/pios_ppm.h b/flight/PiOS/inc/pios_ppm.h index a7c809f70..d75868f4e 100644 --- a/flight/PiOS/inc/pios_ppm.h +++ b/flight/PiOS/inc/pios_ppm.h @@ -30,8 +30,4 @@ #ifndef PIOS_PPM_H #define PIOS_PPM_H -/* Public Functions */ -extern void PIOS_PPM_Init(void); -extern int32_t PIOS_PPM_Get(int8_t Channel); - #endif /* PIOS_PPM_H */ diff --git a/flight/PiOS/inc/pios_ppm_priv.h b/flight/PiOS/inc/pios_ppm_priv.h index 5fdcd73e1..cd4ca40a6 100644 --- a/flight/PiOS/inc/pios_ppm_priv.h +++ b/flight/PiOS/inc/pios_ppm_priv.h @@ -59,6 +59,10 @@ extern uint8_t pios_ppm_num_channels; extern const struct pios_ppm_cfg pios_ppm_cfg; extern const struct pios_ppmsv_cfg pios_ppmsv_cfg; +extern const struct pios_rcvr_driver pios_ppm_rcvr_driver; + +extern void PIOS_PPM_Init(void); + #endif /* PIOS_PPM_PRIV_H */ /** diff --git a/flight/PiOS/inc/pios_pwm.h b/flight/PiOS/inc/pios_pwm.h index 32635c8b8..3552319c9 100644 --- a/flight/PiOS/inc/pios_pwm.h +++ b/flight/PiOS/inc/pios_pwm.h @@ -30,9 +30,4 @@ #ifndef PIOS_PWM_H #define PIOS_PWM_H -/* Public Functions */ -extern void PIOS_PWM_Init(void); -extern int32_t PIOS_PWM_Get(int8_t Channel); -//extern void PIOS_PWM_irq_handler(TIM_TypeDef * timer); - #endif /* PIOS_PWM_H */ diff --git a/flight/PiOS/inc/pios_pwm_priv.h b/flight/PiOS/inc/pios_pwm_priv.h index f2119e63c..23d7d4818 100644 --- a/flight/PiOS/inc/pios_pwm_priv.h +++ b/flight/PiOS/inc/pios_pwm_priv.h @@ -57,6 +57,10 @@ extern void PIOS_PWM_irq_handler(TIM_TypeDef * timer); extern uint8_t pios_pwm_num_channels; extern const struct pios_pwm_cfg pios_pwm_cfg; +extern const struct pios_rcvr_driver pios_pwm_rcvr_driver; + +extern void PIOS_PWM_Init(void); + #endif /* PIOS_PWM_PRIV_H */ /** diff --git a/flight/PiOS/inc/pios_sbus.h b/flight/PiOS/inc/pios_sbus.h index d30b1432d..868fedd70 100644 --- a/flight/PiOS/inc/pios_sbus.h +++ b/flight/PiOS/inc/pios_sbus.h @@ -33,9 +33,6 @@ /* Global Types */ /* Public Functions */ -extern void PIOS_SBUS_Init(void); -extern int16_t PIOS_SBUS_Get(int8_t Channel); -extern void SBUS_IRQHandler(uint32_t usart_id); #endif /* PIOS_SBUS_H */ diff --git a/flight/PiOS/inc/pios_sbus_priv.h b/flight/PiOS/inc/pios_sbus_priv.h index aa626dbdd..cb73b24cd 100644 --- a/flight/PiOS/inc/pios_sbus_priv.h +++ b/flight/PiOS/inc/pios_sbus_priv.h @@ -76,9 +76,16 @@ struct pios_sbus_cfg { GPIO_InitTypeDef gpio_inv_init; BitAction gpio_inv_enable; }; -extern const struct pios_sbus_cfg pios_sbus_cfg; extern void PIOS_SBUS_irq_handler(); +extern void PIOS_SBUSSV_irq_handler(); + +extern uint8_t pios_sbus_num_channels; +extern const struct pios_sbus_cfg pios_sbus_cfg; + +extern const struct pios_rcvr_driver pios_sbus_rcvr_driver; + +extern void PIOS_SBUS_Init(void); #endif /* PIOS_SBUS_PRIV_H */ diff --git a/flight/PiOS/inc/pios_spektrum.h b/flight/PiOS/inc/pios_spektrum.h index 241782e9d..40a995cd1 100644 --- a/flight/PiOS/inc/pios_spektrum.h +++ b/flight/PiOS/inc/pios_spektrum.h @@ -34,11 +34,8 @@ /* Global Types */ /* Public Functions */ -extern void PIOS_SPEKTRUM_Init(void); extern uint8_t PIOS_SPEKTRUM_Bind(void); extern int32_t PIOS_SPEKTRUM_Decode(uint8_t b); -extern int16_t PIOS_SPEKTRUM_Get(int8_t Channel); -extern void SPEKTRUM_IRQHandler(uint32_t usart_id); #endif /* PIOS_SPEKTRUM_H */ diff --git a/flight/PiOS/inc/pios_spektrum_priv.h b/flight/PiOS/inc/pios_spektrum_priv.h index b5dd384d1..5b1ac616e 100644 --- a/flight/PiOS/inc/pios_spektrum_priv.h +++ b/flight/PiOS/inc/pios_spektrum_priv.h @@ -45,10 +45,15 @@ struct pios_spektrum_cfg { }; extern void PIOS_SPEKTRUM_irq_handler(); +extern void PIOS_SPEKTRUMSV_irq_handler(); extern uint8_t pios_spektrum_num_channels; extern const struct pios_spektrum_cfg pios_spektrum_cfg; +extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver; + +extern void PIOS_SPEKTRUM_Init(void); + #endif /* PIOS_PWM_PRIV_H */ /**