rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

rcvr: convert PWM, PPM, SBUS and Spektrum to use PIOS_RCVR

Browse Source 
All receivers now fall under the same driver API provided
by pios_rcvr.c.

This is part of a larger sequence of commits that will
switch the receiver selection over to boot time dynamic
configuration via UAVObjects.
This commit is contained in:
Stacey Sheldon 2011-06-25 09:27:28 -04:00
parent e77795dc28
commit 740b5f1584
21 changed files with 979 additions and 969 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flight/CopterControl/Makefile
View File

@ -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

234
flight/CopterControl/System/inc/pios_config.h
View File

@ -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 */
/**
* @}
* @}
*/

83
flight/CopterControl/System/pios_board.c
View File

@ -469,13 +469,13 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
},
};
#include <pios_spektrum_priv.h>
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 <pios_spektrum_priv.h>
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 <pios_sbus_priv.h>
@ -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);

1253
flight/Modules/ManualControl/manualcontrol.c
View File

@ -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);
}
/**
* @}
* @}
*/

1
flight/OpenPilot/Makefile
View File

@ -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

2
flight/OpenPilot/System/inc/pios_config.h
View File

@ -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)

79
flight/OpenPilot/System/pios_board.c
View File

@ -497,13 +497,13 @@ const struct pios_usart_cfg pios_usart_spektrum_cfg = {
},
};
#include <pios_spektrum_priv.h>
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 <pios_spektrum_priv.h>
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);

21
flight/PiOS/Boards/STM32103CB_CC_Rev1.h
View File

@ -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

30
flight/PiOS/Boards/STM3210E_OP.h
View File

@ -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

14
flight/PiOS/STM32F10x/pios_ppm.c
View File

@ -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];
}
/**

151
flight/PiOS/STM32F10x/pios_pwm.c
View File

@ -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
/**

17
flight/PiOS/STM32F10x/pios_sbus.c
View File

@ -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) {

25
flight/PiOS/STM32F10x/pios_spektrum.c
View File

@ -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;i<PIOS_SPEKTRUM_NUM_INPUTS;i++)
{
CaptureValue[i] = CaptureValueTemp[i];
}
@ -224,7 +231,7 @@ int32_t PIOS_SPEKTRUM_Decode(uint8_t b)
}
/* Interrupt handler for USART */
void SPEKTRUM_IRQHandler(uint32_t usart_id) {
void PIOS_SPEKTRUM_irq_handler(uint32_t usart_id) {
/* by always reading DR after SR make sure to clear any error interrupts */
volatile uint16_t sr = pios_spektrum_cfg.pios_usart_spektrum_cfg->regs->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;
}

4
flight/PiOS/inc/pios_ppm.h
View File

@ -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 */

4
flight/PiOS/inc/pios_ppm_priv.h
View File

@ -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 */
/**

5
flight/PiOS/inc/pios_pwm.h
View File

@ -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 */

4
flight/PiOS/inc/pios_pwm_priv.h
View File

@ -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 */
/**

3
flight/PiOS/inc/pios_sbus.h
View File

@ -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 */

9
flight/PiOS/inc/pios_sbus_priv.h
View File

@ -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 */

3
flight/PiOS/inc/pios_spektrum.h
View File

@ -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 */

5
flight/PiOS/inc/pios_spektrum_priv.h
View File

@ -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 */
/**