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

Merge branch 'next' into ins

Browse Source 
Conflicts:
	flight/Libraries/inc/fifo_buffer.h
	flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld
	flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj
This commit is contained in:
James Cotton 2011-08-21 00:47:43 -05:00
commit e1afc9a19a
87 changed files with 3153 additions and 1436 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
HISTORY.txt
View File

@ -1,5 +1,18 @@
Short summary of changes. For a complete list see the git log.
2011-08-10
Added Camera Stabilization and a gui to configure this. This is a software
selectable module from the GUI. However, a restart is required to make it
active. The GUI does not currently expose the configuration for using the
transmitter to change the view angle but this is supported by the hardware.
2011-08-10
By default a lot of diagnostic objects that were enabled by default are now
disabled in the build. This include TaskInfo (and all the FreeRTOS options
that provide that debugging information). Also MixerStatus, I2CStatus,
WatchdogStatus and RateDesired. These can be reenabled for debugging with
-DDIAGNOSTICS.
2011-08-04
Fixed packaging aesthetic issues. Also avoid runtime issues on OSX Lion by
disabling the ModelView and Notify plugins for now (sorry).

10
MILESTONES.txt
View File

@ -137,6 +137,16 @@ C: Mat Wellington
D: July 2011
V: http://www.youtube.com/watch?v=YE4Fd9vdg1I
M: First CopterControl Flybared Heli inverted flight (2:33)
C: Maxim Izergin (Maximus43)
D: August 2011
V: http://www.youtube.com/watch?v=8SrfIS7OkB4
M: First CopterControl Flybared Heli funnel (4:18), loop (5:35)
C: Sergey Solodennikov (alconaft43)
D: August 2011
V: http://www.youtube.com/watch?v=8SrfIS7OkB4
M: First Altitude Hold using Sonar
C:
D:

30
flight/CopterControl/Makefile
View File

@ -37,6 +37,9 @@ OUTDIR := $(TOP)/build/$(TARGET)
# Set to YES to compile for debugging
DEBUG ?= NO
# Include objects that are just nice information to show
DIAGNOSTICS ?= NO
# Set to YES to build a FW version that will erase all flash memory
ERASE_FLASH ?= NO
# Set to YES to use the Servo output pins for debugging via scope or logic analyser
@ -62,7 +65,9 @@ endif
FLASH_TOOL = OPENOCD
# List of modules to include
MODULES = Telemetry Attitude Stabilization Actuator ManualControl FirmwareIAP
MODULES = Attitude Stabilization Actuator ManualControl FirmwareIAP CameraStab
# Telemetry must be last to grab the optional modules
MODULES += Telemetry
# Paths
OPSYSTEM = ./System
@ -125,7 +130,6 @@ SRC += $(OPSYSTEM)/taskmonitor.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/uavobjectsinit_linker.c
SRC += $(OPSYSTEM)/pios_usb_hid_desc.c
else
## TESTCODE
@ -153,21 +157,21 @@ SRC += $(OPUAVSYNTHDIR)/actuatorsettings.c
SRC += $(OPUAVSYNTHDIR)/attituderaw.c
SRC += $(OPUAVSYNTHDIR)/attitudeactual.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolcommand.c
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/i2cstats.c
SRC += $(OPUAVSYNTHDIR)/watchdogstatus.c
SRC += $(OPUAVSYNTHDIR)/telemetrysettings.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
SRC += $(OPUAVSYNTHDIR)/manualcontrolsettings.c
SRC += $(OPUAVSYNTHDIR)/mixersettings.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c
SRC += $(OPUAVSYNTHDIR)/attitudesettings.c
SRC += $(OPUAVSYNTHDIR)/camerastabsettings.c
SRC += $(OPUAVSYNTHDIR)/cameradesired.c
SRC += $(OPUAVSYNTHDIR)/hwsettings.c
#${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c}
#SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c}
# Cant use until i can automatically generate list of UAVObjects
#SRC += ${OUTDIR}/InitObjects.c
SRC += $(OPUAVSYNTHDIR)/taskinfo.c
SRC += $(OPUAVSYNTHDIR)/mixerstatus.c
SRC += $(OPUAVSYNTHDIR)/ratedesired.c
endif
## PIOS Hardware (STM32F10x)
@ -417,6 +421,10 @@ ifeq ($(DEBUG),YES)
CFLAGS = -DDEBUG
endif
ifeq ($(DIAGNOSTICS),YES)
CFLAGS = -DDIAGNOSTICS
endif
CFLAGS += -g$(DEBUGF)
CFLAGS += -O$(OPT)
CFLAGS += -mcpu=$(MCU)
@ -425,7 +433,7 @@ CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
#CFLAGS += -fno-cprop-registers -fno-defer-pop -fno-guess-branch-probability -fno-section-anchors
#CFLAGS += -fno-if-conversion -fno-if-conversion2 -fno-ipa-pure-const -fno-ipa-reference -fno-merge-constants
#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename
#CFLAGS += -fno-split-wide-types -fno-tree-ccp -fno-tree-ch -fno-tree-copy-prop -fno-tree-copyrename
#CFLAGS += -fno-tree-dce -fno-tree-dominator-opts -fno-tree-dse -fno-tree-fre -fno-tree-sink -fno-tree-sra
#CFLAGS += -fno-tree-ter
#CFLAGS += -g$(DEBUGF) -DDEBUG
@ -504,7 +512,7 @@ endif
endif
# Generate code for PyMite
#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py)
#$(OUTDIR)/pmlib_img.c $(OUTDIR)/pmlib_nat.c $(OUTDIR)/pmlibusr_img.c $(OUTDIR)/pmlibusr_nat.c $(OUTDIR)/pmfeatures.h: $(wildcard $(PYMITELIB)/*.py) $(wildcard $(PYMITEPLAT)/*.py) $(wildcard $(FLIGHTPLANLIB)/*.py) $(wildcard $(FLIGHTPLANS)/*.py)
# @echo $(MSG_PYMITEINIT) $(call toprel, $@)
# @$(PYTHON) $(PYMITETOOLS)/pmImgCreator.py -f $(PYMITEPLAT)/pmfeatures.py -c -s --memspace=flash -o $(OUTDIR)/pmlib_img.c --native-file=$(OUTDIR)/pmlib_nat.c $(PYMITELIB)/list.py $(PYMITELIB)/dict.py $(PYMITELIB)/__bi.py $(PYMITELIB)/sys.py $(PYMITELIB)/string.py $(wildcard $(FLIGHTPLANLIB)/*.py)
# @$(PYTHON) $(PYMITETOOLS)/pmGenPmFeatures.py $(PYMITEPLAT)/pmfeatures.py > $(OUTDIR)/pmfeatures.h

10
flight/CopterControl/System/alarms.c
View File

@ -41,10 +41,12 @@ static xSemaphoreHandle lock;
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
/**
* Initialize the alarms library
* Initialize the alarms library
*/
int32_t AlarmsInitialize(void)
{
SystemAlarmsInitialize();
lock = xSemaphoreCreateRecursiveMutex();
//do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that
//AlarmsClearAll();
@ -56,7 +58,7 @@ int32_t AlarmsInitialize(void)
* Set an alarm
* @param alarm The system alarm to be modified
* @param severity The alarm severity
* @return 0 if success, -1 if an error
* @return 0 if success, -1 if an error
*/
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
{
@ -151,7 +153,7 @@ void AlarmsClearAll()
/**
* Check if there are any alarms with the given or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasWarnings()
{
@ -208,5 +210,5 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
/**
* @}
* @}
*/
*/

204
flight/CopterControl/System/coptercontrol.c
View File

@ -1,97 +1,107 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @brief These files are the core system files of OpenPilot.
* They are the ground layer just above PiOS. In practice, OpenPilot actually starts
* in the main() function of openpilot.c
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @brief This is where the OP firmware starts. Those files also define the compile-time
* options of the firmware.
* @{
* @file openpilot.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sets up and runs main OpenPilot tasks.
* @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
*/
/* OpenPilot Includes */
#include "openpilot.h"
#include "uavobjectsinit.h"
#include "systemmod.h"
/* Task Priorities */
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
/* Global Variables */
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void Stack_Change(void);
/**
* OpenPilot Main function:
*
* Initialize PiOS<BR>
* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller)
* If something goes wrong, blink LED1 and LED2 every 100ms
*
*/
int main()
{
/* NOTE: Do NOT modify the following start-up sequence */
/* Any new initialization functions should be added in OpenPilotInit() */
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Architecture dependant Hardware and
* core subsystem initialisation
* (see pios_board.c for your arch)
* */
PIOS_Board_Init();
/* Initialize modules */
MODULE_INITIALISE_ALL
/* swap the stack to use the IRQ stack */
Stack_Change();
/* Start the FreeRTOS scheduler which should never returns.*/
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* Do some indication to user that something bad just happened */
PIOS_LED_Off(LED1); \
for(;;) { \
PIOS_LED_Toggle(LED1); \
PIOS_DELAY_WaitmS(100); \
};
return 0;
}
/**
* @}
* @}
*/
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @brief These files are the core system files of OpenPilot.
* They are the ground layer just above PiOS. In practice, OpenPilot actually starts
* in the main() function of openpilot.c
* @{
* @addtogroup OpenPilotCore OpenPilot Core
* @brief This is where the OP firmware starts. Those files also define the compile-time
* options of the firmware.
* @{
* @file openpilot.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Sets up and runs main OpenPilot tasks.
* @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
*/
/* OpenPilot Includes */
#include "openpilot.h"
#include "uavobjectsinit.h"
#include "hwsettings.h"
#include "camerastab.h"
#include "systemmod.h"
/* Task Priorities */
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
/* Global Variables */
/* Prototype of PIOS_Board_Init() function */
extern void PIOS_Board_Init(void);
extern void Stack_Change(void);
/**
* OpenPilot Main function:
*
* Initialize PiOS<BR>
* Create the "System" task (SystemModInitializein Modules/System/systemmod.c) <BR>
* Start FreeRTOS Scheduler (vTaskStartScheduler) (Now handled by caller)
* If something goes wrong, blink LED1 and LED2 every 100ms
*
*/
int main()
{
/* NOTE: Do NOT modify the following start-up sequence */
/* Any new initialization functions should be added in OpenPilotInit() */
/* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init();
/* Architecture dependant Hardware and
* core subsystem initialisation
* (see pios_board.c for your arch)
* */
PIOS_Board_Init();
/* Initialize modules */
MODULE_INITIALISE_ALL
/* Optional module initialization. This code might want to go somewhere else as
* it grows */
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
HwSettingsOptionalModulesGet(optionalModules);
if(optionalModules[HWSETTINGS_OPTIONALMODULES_CAMERASTABILIZATION] == HWSETTINGS_OPTIONALMODULES_ENABLED) {
CameraStabInitialize();
}
/* swap the stack to use the IRQ stack */
Stack_Change();
/* Start the FreeRTOS scheduler which should never returns.*/
vTaskStartScheduler();
/* If all is well we will never reach here as the scheduler will now be running. */
/* Do some indication to user that something bad just happened */
PIOS_LED_Off(LED1); \
for(;;) { \
PIOS_LED_Toggle(LED1); \
PIOS_DELAY_WaitmS(100); \
};
return 0;
}
/**
* @}
* @}
*/

9
flight/CopterControl/System/inc/FreeRTOSConfig.h
View File

@ -39,7 +39,6 @@
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_COUNTING_SEMAPHORES 0
#define configUSE_ALTERNATIVE_API 0
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configQUEUE_REGISTRY_SIZE 10
/* Co-routine definitions. */
@ -76,7 +75,9 @@ NVIC value of 255. */
#endif
/* Enable run time stats collection */
//#if defined(DEBUG)
#if defined(DIAGNOSTICS)
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\
@ -85,7 +86,9 @@ do {\
(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\
} while(0)
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
//#endif
#else
#define configCHECK_FOR_STACK_OVERFLOW 1
#endif
/**

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

@ -99,10 +99,13 @@
#define PIOS_SYSTEM_STACK_SIZE 460
#define PIOS_STABILIZATION_STACK_SIZE 524
#define PIOS_TELEM_STACK_SIZE 500
#define PIOS_EVENTDISPATCHER_STACK_SIZE 96
#define PIOS_EVENTDISPATCHER_STACK_SIZE 130
#define IDLE_COUNTS_PER_SEC_AT_NO_LOAD 1995998
//#define PIOS_QUATERNION_STABILIZATION
// This can't be too high to stop eventdispatcher thread overflowing
#define PIOS_EVENTDISAPTCHER_QUEUE 10
#endif /* PIOS_CONFIG_H */
/**
* @}

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

@ -907,8 +907,10 @@ void PIOS_Board_Init(void) {
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
HwSettingsInitialize();
ManualControlSettingsInitialize();
#if defined(PIOS_INCLUDE_RTC)
/* Initialize the real-time clock and its associated tick */
PIOS_RTC_Init(&pios_rtc_main_cfg);

1
flight/CopterControl/System/pios_board_posix.c
View File

@ -42,7 +42,6 @@ void PIOS_Board_Init(void) {
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
/* Initialize the alarms library */
AlarmsInitialize();

5
flight/CopterControl/System/taskmonitor.c
View File

@ -46,7 +46,10 @@ int32_t TaskMonitorInitialize(void)
{
lock = xSemaphoreCreateRecursiveMutex();
memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
lastMonitorTime = 0;
#if defined(DIAGNOSTICS)
lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
#endif
return 0;
}
@ -73,6 +76,7 @@ int32_t TaskMonitorAdd(TaskInfoRunningElem task, xTaskHandle handle)
*/
void TaskMonitorUpdateAll(void)
{
#if defined(DIAGNOSTICS)
TaskInfoData data;
int n;
@ -123,4 +127,5 @@ void TaskMonitorUpdateAll(void)
// Done
xSemaphoreGiveRecursive(lock);
#endif
}

9
flight/Modules/AHRSComms/ahrs_comms.c
View File

@ -52,6 +52,8 @@
#include "ahrs_comms.h"
#include "ahrs_spi_comm.h"
#include "ahrsstatus.h"
#include "ahrscalibration.h"
// Private constants
#define STACK_SIZE configMINIMAL_STACK_SIZE-128
@ -71,7 +73,7 @@ static void ahrscommsTask(void *parameters);
*/
int32_t AHRSCommsStart(void)
{
// Start main task
// Start main task
xTaskCreate(ahrscommsTask, (signed char *)"AHRSComms", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_AHRSCOMMS, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_AHRS);
@ -85,6 +87,11 @@ int32_t AHRSCommsStart(void)
*/
int32_t AHRSCommsInitialize(void)
{
AhrsStatusInitialize();
AHRSCalibrationInitialize();
AttitudeRawInitialize();
VelocityActualInitialize();
PositionActualInitialize();
return 0;
}

102
flight/Modules/Actuator/actuator.c
View File

@ -41,6 +41,7 @@
#include "flightstatus.h"
#include "mixersettings.h"
#include "mixerstatus.h"
#include "cameradesired.h"
// Private constants
@ -108,9 +109,17 @@ int32_t ActuatorInitialize()
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
ActuatorSettingsInitialize();
ActuatorDesiredInitialize();
MixerSettingsInitialize();
ActuatorCommandInitialize();
#if defined(DIAGNOSTICS)
MixerStatusInitialize();
#endif
// Listen for ExampleObject1 updates
ActuatorDesiredConnectQueue(queue);
// If settings change, update the output rate
ActuatorSettingsConnectCallback(actuator_update_rate);
@ -160,7 +169,7 @@ static void actuatorTask(void* parameters)
// Main task loop
lastSysTime = xTaskGetTickCount();
while (1)
{
{
PIOS_WDG_UpdateFlag(PIOS_WDG_ACTUATOR);
// Wait until the ActuatorDesired object is updated, if a timeout then go to failsafe
@ -177,11 +186,13 @@ static void actuatorTask(void* parameters)
lastSysTime = thisSysTime;
FlightStatusGet(&flightStatus);
MixerStatusGet(&mixerStatus);
MixerSettingsGet (&mixerSettings);
ActuatorDesiredGet(&desired);
ActuatorCommandGet(&command);
#if defined(DIAGNOSTICS)
MixerStatusGet(&mixerStatus);
#endif
ActuatorSettingsMotorsSpinWhileArmedGet(&MotorsSpinWhileArmed);
ActuatorSettingsChannelMaxGet(ChannelMax);
ActuatorSettingsChannelMinGet(ChannelMin);
@ -196,7 +207,7 @@ static void actuatorTask(void* parameters)
nMixers ++;
}
}
if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers.
if((nMixers < 2) && !ActuatorCommandReadOnly(dummy)) //Nothing can fly with less than two mixers.
{
setFailsafe(); // So that channels like PWM buzzer keep working
continue;
@ -207,7 +218,7 @@ static void actuatorTask(void* parameters)
bool armed = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED;
bool positiveThrottle = desired.Throttle >= 0.00;
bool spinWhileArmed = MotorsSpinWhileArmed == ACTUATORSETTINGS_MOTORSSPINWHILEARMED_TRUE;
float curve1 = MixerCurve(desired.Throttle,mixerSettings.ThrottleCurve1);
//The source for the secondary curve is selectable
float curve2 = 0;
@ -233,11 +244,11 @@ static void actuatorTask(void* parameters)
case MIXERSETTINGS_CURVE2SOURCE_ACCESSORY5:
if(AccessoryDesiredInstGet(mixerSettings.Curve2Source - MIXERSETTINGS_CURVE2SOURCE_ACCESSORY0,&accessory) == 0)
curve2 = MixerCurve(accessory.AccessoryVal,mixerSettings.ThrottleCurve2);
else
curve2 = 0;
else
curve2 = 0;
break;
}
for(int ct=0; ct < MAX_MIX_ACTUATORS; ct++)
{
if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_DISABLED) {
@ -246,53 +257,82 @@ static void actuatorTask(void* parameters)
command.Channel[ct] = 0;
continue;
}
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
if((mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) || (mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_SERVO))
status[ct] = ProcessMixer(ct, curve1, curve2, &mixerSettings, &desired, dT);
else
status[ct] = -1;
// Motors have additional protection for when to be on
if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
if(mixers[ct].type == MIXERSETTINGS_MIXER1TYPE_MOTOR) {
// If not armed or motors aren't meant to spin all the time
if( !armed ||
(!spinWhileArmed && !positiveThrottle))
{
filterAccumulator[ct] = 0;
lastResult[ct] = 0;
lastResult[ct] = 0;
status[ct] = -1; //force min throttle
}
// If armed meant to keep spinning,
}
// If armed meant to keep spinning,
else if ((spinWhileArmed && !positiveThrottle) ||
(status[ct] < 0) )
status[ct] = 0;
status[ct] = 0;
}
// If an accessory channel is selected for direct bypass mode
// In this configuration the accessory channel is scaled and mapped
// directly to output. Note: THERE IS NO SAFETY CHECK HERE FOR ARMING
// these also will not be updated in failsafe mode. I'm not sure what
// these also will not be updated in failsafe mode. I'm not sure what
// the correct behavior is since it seems domain specific. I don't love
// this code
if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY2))
if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_ACCESSORY0) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_ACCESSORY5))
{
if(AccessoryDesiredInstGet(mixers[ct].type - MIXERSETTINGS_MIXER1TYPE_ACCESSORY0,&accessory) == 0)
status[ct] = accessory.AccessoryVal;
else
status[ct] = -1;
}
if( (mixers[ct].type >= MIXERSETTINGS_MIXER1TYPE_CAMERAROLL) &&
(mixers[ct].type <= MIXERSETTINGS_MIXER1TYPE_CAMERAYAW))
{
CameraDesiredData cameraDesired;
if( CameraDesiredGet(&cameraDesired) == 0 ) {
switch(mixers[ct].type) {
case MIXERSETTINGS_MIXER1TYPE_CAMERAROLL:
status[ct] = cameraDesired.Roll;
break;
case MIXERSETTINGS_MIXER1TYPE_CAMERAPITCH:
status[ct] = cameraDesired.Pitch;
break;
case MIXERSETTINGS_MIXER1TYPE_CAMERAYAW:
status[ct] = cameraDesired.Yaw;
break;
default:
break;
}
}
else
status[ct] = -1;
}
command.Channel[ct] = scaleChannel(status[ct],
ChannelMax[ct],
ChannelMin[ct],
ChannelNeutral[ct]);
}
#if defined(DIAGNOSTICS)
MixerStatusSet(&mixerStatus);
#endif
// Store update time
command.UpdateTime = 1000*dT;
if(1000*dT > command.MaxUpdateTime)
command.MaxUpdateTime = 1000*dT;
// Update output object
ActuatorCommandSet(&command);
// Update in case read only (eg. during servo configuration)
@ -300,7 +340,7 @@ static void actuatorTask(void* parameters)
// Update servo outputs
bool success = true;
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
{
success &= set_channel(n, command.Channel[n]);
@ -309,7 +349,7 @@ static void actuatorTask(void* parameters)
if(!success) {
command.NumFailedUpdates++;
ActuatorCommandSet(&command);
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
AlarmsSet(SYSTEMALARMS_ALARM_ACTUATOR, SYSTEMALARMS_ALARM_CRITICAL);
}
}
@ -463,7 +503,7 @@ static void setFailsafe()
// Reset ActuatorCommand to safe values
for (int n = 0; n < ACTUATORCOMMAND_CHANNEL_NUMELEM; ++n)
{
if(mixers[n].type == MIXERSETTINGS_MIXER1TYPE_MOTOR)
{
Channel[n] = ChannelMin[n];
@ -510,10 +550,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
}
#else
static bool set_channel(uint8_t mixer_channel, uint16_t value) {
ActuatorSettingsData settings;
ActuatorSettingsGet(&settings);
switch(settings.ChannelType[mixer_channel]) {
case ACTUATORSETTINGS_CHANNELTYPE_PWMALARMBUZZER: {
// This is for buzzers that take a PWM input
@ -565,7 +605,7 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
PIOS_Servo_Set(settings.ChannelAddr[mixer_channel], value);
return true;
#if defined(PIOS_INCLUDE_I2C_ESC)
case ACTUATORSETTINGS_CHANNELTYPE_MK:
case ACTUATORSETTINGS_CHANNELTYPE_MK:
return PIOS_SetMKSpeed(settings.ChannelAddr[mixer_channel],value);
case ACTUATORSETTINGS_CHANNELTYPE_ASTEC4:
return PIOS_SetAstec4Speed(settings.ChannelAddr[mixer_channel],value);
@ -573,10 +613,10 @@ static bool set_channel(uint8_t mixer_channel, uint16_t value) {
#endif
default:
return false;
}
}
return false;
}
#endif

6
flight/Modules/Altitude/altitude.c
View File

@ -69,6 +69,12 @@ static void altitudeTask(void *parameters);
*/
int32_t AltitudeStart()
{
BaroAltitudeInitialize();
#if defined(PIOS_INCLUDE_HCSR04)
SonarAltitudeInitialze();
#endif
// Start main task
xTaskCreate(altitudeTask, (signed char *)"Altitude", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_ALTITUDE, taskHandle);

43
flight/Modules/Attitude/attitude.c
View File

@ -76,7 +76,7 @@ static void AttitudeTask(void *parameters);
static float gyro_correct_int[3] = {0,0,0};
static xQueueHandle gyro_queue;
static void updateSensors(AttitudeRawData *);
static int8_t updateSensors(AttitudeRawData *);
static void updateAttitude(AttitudeRawData *);
static void settingsUpdatedCb(UAVObjEvent * objEv);
@ -112,6 +112,10 @@ int32_t AttitudeStart(void)
*/
int32_t AttitudeInitialize(void)
{
AttitudeActualInitialize();
AttitudeRawInitialize();
AttitudeSettingsInitialize();
// Initialize quaternion
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
@ -162,14 +166,19 @@ static void AttitudeTask(void *parameters)
// Keep flash CS pin high while talking accel
PIOS_FLASH_DISABLE;
PIOS_ADXL345_Init();
// Set critical error and wait until the accel is producing data
while(PIOS_ADXL345_FifoElements() == 0) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_CRITICAL);
PIOS_WDG_UpdateFlag(PIOS_WDG_ATTITUDE);
}
// Force settings update to make sure rotation loaded
settingsUpdatedCb(AttitudeSettingsHandle());
// Main task loop
while (1) {
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
@ -197,14 +206,24 @@ static void AttitudeTask(void *parameters)
AttitudeRawData attitudeRaw;
AttitudeRawGet(&attitudeRaw);
updateSensors(&attitudeRaw);
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
if(updateSensors(&attitudeRaw) != 0)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
else {
// Only update attitude when sensor data is good
updateAttitude(&attitudeRaw);
AttitudeRawSet(&attitudeRaw);
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
}
}
}
static void updateSensors(AttitudeRawData * attitudeRaw)
/**
* Get an update from the sensors
* @param[in] attitudeRaw Populate the UAVO instead of saving right here
* @return 0 if successfull, -1 if not
*/
static int8_t updateSensors(AttitudeRawData * attitudeRaw)
{
struct pios_adxl345_data accel_data;
float gyro[4];
@ -212,9 +231,12 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
// Only wait the time for two nominal updates before setting an alarm
if(xQueueReceive(gyro_queue, (void * const) gyro, UPDATE_RATE * 2) == errQUEUE_EMPTY) {
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE, SYSTEMALARMS_ALARM_ERROR);
return;
return -1;
}
// No accel data available
if(PIOS_ADXL345_FifoElements() == 0)
return -1;
// First sample is temperature
attitudeRaw->gyros[ATTITUDERAW_GYROS_X] = -(gyro[1] - GYRO_NEUTRAL) * gyroGain;
@ -270,6 +292,8 @@ static void updateSensors(AttitudeRawData * attitudeRaw)
// Because most crafts wont get enough information from gravity to zero yaw gyro, we try
// and make it average zero (weakly)
gyro_correct_int[2] += - attitudeRaw->gyros[ATTITUDERAW_GYROS_Z] * yawBiasRate;
return 0;
}
static void updateAttitude(AttitudeRawData * attitudeRaw)
@ -307,6 +331,7 @@ static void updateAttitude(AttitudeRawData * attitudeRaw)
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi;
gyro_correct_int[1] += accel_err[1] * accelKi;
//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;
// Correct rates based on error, integral component dealt with in updateSensors

3
flight/Modules/Battery/battery.c
View File

@ -79,6 +79,9 @@ MODULE_INITCALL(BatteryInitialize, 0)
int32_t BatteryInitialize(void)
{
BatteryStateInitialze();
BatterySettingsInitialize();
static UAVObjEvent ev;
memset(&ev,0,sizeof(UAVObjEvent));

141
flight/Modules/CameraStab/camerastab.c Normal file
View File

@ -0,0 +1,141 @@
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup CameraStab Camera Stabilization Module
* @brief Camera stabilization module
* Updates accessory outputs with values appropriate for camera stabilization
* @{
*
* @file camerastab.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Stabilize camera against the roll pitch and yaw of aircraft
*
* @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
*/
/**
* Output object: Accessory
*
* This module will periodically calculate the output values for stabilizing the camera
*
* UAVObjects are automatically generated by the UAVObjectGenerator from
* the object definition XML file.
*
* Modules have no API, all communication to other modules is done through UAVObjects.
* However modules may use the API exposed by shared libraries.
* See the OpenPilot wiki for more details.
* http://www.openpilot.org/OpenPilot_Application_Architecture
*
*/
#include "openpilot.h"
#include "accessorydesired.h"
#include "attitudeactual.h"
#include "camerastabsettings.h"
#include "cameradesired.h"
//
// Configuration
//
#define SAMPLE_PERIOD_MS 10
// Private types
// Private variables
// Private functions
static void attitudeUpdated(UAVObjEvent* ev);
static float bound(float val);
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t CameraStabInitialize(void)
{
static UAVObjEvent ev;
ev.obj = AttitudeActualHandle();
ev.instId = 0;
ev.event = 0;
CameraStabSettingsInitialize();
CameraDesiredInitialize();
EventPeriodicCallbackCreate(&ev, attitudeUpdated, SAMPLE_PERIOD_MS / portTICK_RATE_MS);
return 0;
}
static void attitudeUpdated(UAVObjEvent* ev)
{
if (ev->obj != AttitudeActualHandle())
return;
float attitude;
float output;
AccessoryDesiredData accessory;
CameraStabSettingsData cameraStab;
CameraStabSettingsGet(&cameraStab);
// Read any input channels
float inputs[3] = {0,0,0};
if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] != CAMERASTABSETTINGS_INPUTS_NONE) {
if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_ROLL] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
inputs[0] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_ROLL];
}
if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] != CAMERASTABSETTINGS_INPUTS_NONE) {
if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_PITCH] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
inputs[1] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_PITCH];
}
if(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] != CAMERASTABSETTINGS_INPUTS_NONE) {
if(AccessoryDesiredInstGet(cameraStab.Inputs[CAMERASTABSETTINGS_INPUTS_YAW] - CAMERASTABSETTINGS_INPUTS_ACCESSORY0, &accessory) == 0)
inputs[2] = accessory.AccessoryVal * cameraStab.InputRange[CAMERASTABSETTINGS_INPUTRANGE_YAW];
}
// Set output channels
AttitudeActualRollGet(&attitude);
output = bound((attitude + inputs[0]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_ROLL]);
CameraDesiredRollSet(&output);
AttitudeActualPitchGet(&attitude);
output = bound((attitude + inputs[1]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_PITCH]);
CameraDesiredPitchSet(&output);
AttitudeActualYawGet(&attitude);
output = bound((attitude + inputs[2]) / cameraStab.OutputRange[CAMERASTABSETTINGS_OUTPUTRANGE_YAW]);
CameraDesiredYawSet(&output);
}
float bound(float val)
{
return (val > 1) ? 1 :
(val < -1) ? -1 :
val;
}
/**
* @}
*/
/**
* @}
*/

87
flight/UAVObjects/uavobjectsinit_linker.c → flight/Modules/CameraStab/inc/camerastab.h
View File

@ -1,45 +1,42 @@
/**
******************************************************************************
*
* @file uavobjectsinit.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Initialize all objects.
* Automatically generated by the UAVObjectGenerator.
*
* @note This is an automatically generated file.
* DO NOT modify manually.
* @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"
/**
* Function used to initialize the first instance of each object.
* This file is automatically updated by the UAVObjectGenerator.
*/
extern initcall_t __uavobj_initcall_start[], __uavobj_initcall_end[];
void UAVObjectsInitializeAll()
{
initcall_t *fn;
int32_t ret;
for (fn = __uavobj_initcall_start; fn < __uavobj_initcall_end; fn++)
ret = (*fn)();
}
/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup BatteryModule Battery Module
* @{
*
* @file battery.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Module to read the battery Voltage and Current periodically and set alarms appropriately.
*
* @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 BATTERY_H
#define BATTERY_H
#include "openpilot.h"
int32_t CameraStabInitialize(void);
#endif // BATTERY_H
/**
* @}
* @}
*/

3
flight/Modules/FirmwareIAP/firmwareiap.c
View File

@ -91,6 +91,9 @@ static void resetTask(UAVObjEvent *);
MODULE_INITCALL(FirmwareIAPInitialize, 0)
int32_t FirmwareIAPInitialize()
{
FirmwareIAPObjInitialize();
const struct pios_board_info * bdinfo = &pios_board_info_blob;
data.BoardType= bdinfo->board_type;

6
flight/Modules/FlightPlan/flightplan.c
View File

@ -74,7 +74,11 @@ int32_t FlightPlanStart()
int32_t FlightPlanInitialize()
{
taskHandle = NULL;
FlightPlanStatusInitialize();
FlightPlanControlInitialize();
FlightPlanSettingsInitialize();
// Listen for object updates
FlightPlanControlConnectCallback(&objectUpdatedCb);

4
flight/Modules/GPS/GPS.c
View File

@ -124,6 +124,10 @@ int32_t GPSStart(void)
*/
int32_t GPSInitialize(void)
{
GPSPositionInitialize();
GPSTimeInitialize();
HomeLocationInitialize();
// TODO: Get gps settings object
gpsPort = PIOS_COM_GPS;

8
flight/Modules/Guidance/guidance.c
View File

@ -97,6 +97,14 @@ int32_t GuidanceStart()
*/
int32_t GuidanceInitialize()
{
GuidanceSettingsInitialize();
PositionDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
NedAccelInitialize();
VelocityDesiredInitialize();
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));

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

@ -90,17 +90,15 @@ static bool validInputRange(int16_t min, int16_t max, uint16_t value);
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode)
/**
* Module initialization
* Module starting
*/
int32_t ManualControlStart()
{
/* 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;
}
@ -110,6 +108,18 @@ int32_t ManualControlStart()
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
if(!assumptions)
return -1;
AccessoryDesiredInitialize();
ManualControlCommandInitialize();
FlightStatusInitialize();
StabilizationDesiredInitialize();
// ManualControlSettingsInitialize(); // this is initialized in
// pios_board.c
return 0;
}
MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
@ -584,4 +594,3 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
* @}
* @}
*/

14
flight/Modules/Stabilization/stabilization.c
View File

@ -40,8 +40,6 @@
#include "attitudeactual.h"
#include "attituderaw.h"
#include "flightstatus.h"
#include "systemsettings.h"
#include "ahrssettings.h"
#include "manualcontrol.h" // Just to get a macro
#include "CoordinateConversions.h"
@ -115,6 +113,11 @@ int32_t StabilizationStart()
int32_t StabilizationInitialize()
{
// Initialize variables
StabilizationSettingsInitialize();
ActuatorDesiredInitialize();
#if defined(DIAGNOSTICS)
RateDesiredInitialize();
#endif
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
@ -147,7 +150,6 @@ static void stabilizationTask(void* parameters)
RateDesiredData rateDesired;
AttitudeActualData attitudeActual;
AttitudeRawData attitudeRaw;
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
SettingsUpdatedCb((UAVObjEvent *) NULL);
@ -175,8 +177,10 @@ static void stabilizationTask(void* parameters)
StabilizationDesiredGet(&stabDesired);
AttitudeActualGet(&attitudeActual);
AttitudeRawGet(&attitudeRaw);
#if defined(DIAGNOSTICS)
RateDesiredGet(&rateDesired);
SystemSettingsGet(&systemSettings);
#endif
#if defined(PIOS_QUATERNION_STABILIZATION)
// Quaternion calculation of error in each axis. Uses more memory.
@ -273,7 +277,9 @@ static void stabilizationTask(void* parameters)
}
uint8_t shouldUpdate = 1;
#if defined(DIAGNOSTICS)
RateDesiredSet(&rateDesired);
#endif
ActuatorDesiredGet(&actuatorDesired);
//Calculate desired command
for(int8_t ct=0; ct< MAX_AXES; ct++)

29
flight/Modules/System/systemmod.c
View File

@ -43,7 +43,9 @@
#include "objectpersistence.h"
#include "flightstatus.h"
#include "systemstats.h"
#include "systemsettings.h"
#include "i2cstats.h"
#include "taskinfo.h"
#include "watchdogstatus.h"
#include "taskmonitor.h"
#include "pios_config.h"
@ -78,11 +80,12 @@ static int32_t stackOverflow;
// Private functions
static void objectUpdatedCb(UAVObjEvent * ev);
static void updateStats();
static void updateI2Cstats();
static void updateWDGstats();
static void updateSystemAlarms();
static void systemTask(void *parameters);
#if defined(DIAGNOSTICS)
static void updateI2Cstats();
static void updateWDGstats();
#endif
/**
* Create the module task.
* \returns 0 on success or -1 if initialization failed
@ -106,6 +109,16 @@ int32_t SystemModStart(void)
int32_t SystemModInitialize(void)
{
// Must registers objects here for system thread because ObjectManager started in OpenPilotInit
SystemSettingsInitialize();
SystemStatsInitialize();
ObjectPersistenceInitialize();
#if defined(DIAGNOSTICS)
TaskInfoInitialize();
I2CStatsInitialize();
WatchdogStatusInitialize();
#endif
SystemModStart();
return 0;
@ -137,9 +150,10 @@ static void systemTask(void *parameters)
// Update the system alarms
updateSystemAlarms();
#if defined(DIAGNOSTICS)
updateI2Cstats();
updateWDGstats();
#endif
// Update the task status object
TaskMonitorUpdateAll();
@ -241,9 +255,7 @@ static void objectUpdatedCb(UAVObjEvent * ev)
/**
* Called periodically to update the I2C statistics
*/
#if defined(ARCH_POSIX) || defined(ARCH_WIN32)
static void updateI2Cstats() {} //Posix and win32 don't have I2C
#else
#if defined(DIAGNOSTICS)
static void updateI2Cstats()
{
#if defined(PIOS_INCLUDE_I2C)
@ -263,7 +275,6 @@ static void updateI2Cstats()
I2CStatsSet(&i2cStats);
#endif
}
#endif
static void updateWDGstats()
{
@ -272,6 +283,8 @@ static void updateWDGstats()
watchdogStatus.ActiveFlags = PIOS_WDG_GetActiveFlags();
WatchdogStatusSet(&watchdogStatus);
}
#endif
/**
* Called periodically to update the system stats

41
flight/Modules/Telemetry/telemetry.c
View File

@ -67,6 +67,7 @@ static uint32_t txErrors;
static uint32_t txRetries;
static TelemetrySettingsData settings;
static uint32_t timeOfLastObjectUpdate;
static UAVTalkConnection uavTalkCon;
// Private functions
static void telemetryTxTask(void *parameters);
@ -88,7 +89,13 @@ static void updateSettings();
*/
int32_t TelemetryStart(void)
{
// Process all registered objects and connect queue for updates
UAVObjIterate(&registerObject);
// Listen to objects of interest
GCSTelemetryStatsConnectQueue(priorityQueue);
TelemetrySettingsConnectQueue(priorityQueue);
// Start telemetry tasks
xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle);
xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle);
@ -111,6 +118,10 @@ int32_t TelemetryStart(void)
int32_t TelemetryInitialize(void)
{
UAVObjEvent ev;
FlightTelemetryStatsInitialize();
GCSTelemetryStatsInitialize();
TelemetrySettingsInitialize();
// Initialize vars
timeOfLastObjectUpdate = 0;
@ -120,25 +131,19 @@ int32_t TelemetryInitialize(void)
#if defined(PIOS_TELEM_PRIORITY_QUEUE)
priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
#endif
// Get telemetry settings object
// Get telemetry settings object
updateSettings();
// Initialise UAVTalk
UAVTalkInitialize(&transmitData);
// Process all registered objects and connect queue for updates
UAVObjIterate(&registerObject);
uavTalkCon = UAVTalkInitialize(&transmitData,256);
// Create periodic event that will be used to update the telemetry stats
txErrors = 0;
txRetries = 0;
memset(&ev, 0, sizeof(UAVObjEvent));
EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS);
// Listen to objects of interest
GCSTelemetryStatsConnectQueue(priorityQueue);
TelemetrySettingsConnectQueue(priorityQueue);
return 0;
}
@ -235,7 +240,7 @@ static void processObjEvent(UAVObjEvent * ev)
if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) {
// Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObject(ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
++retries;
}
// Update stats
@ -246,7 +251,7 @@ static void processObjEvent(UAVObjEvent * ev)
} else if (ev->event == EV_UPDATE_REQ) {
// Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) {
success = UAVTalkSendObjectRequest(ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries;
}
// Update stats
@ -326,7 +331,7 @@ static void telemetryRxTask(void *parameters)
bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), 500);
if (bytes_to_process > 0) {
for (uint8_t i = 0; i < bytes_to_process; i++) {
UAVTalkProcessInputStream(serial_data[i]);
UAVTalkProcessInputStream(uavTalkCon,serial_data[i]);
}
}
} else {
@ -426,8 +431,8 @@ static void updateTelemetryStats()
uint32_t timeNow;
// Get stats
UAVTalkGetStats(&utalkStats);
UAVTalkResetStats();
UAVTalkGetStats(uavTalkCon, &utalkStats);
UAVTalkResetStats(uavTalkCon);
// Get object data
FlightTelemetryStatsGet(&flightStats);

4
flight/OpenPilot/Makefile
View File

@ -135,7 +135,6 @@ SRC += $(OPSYSTEM)/taskmonitor.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectmanager.c
SRC += $(OPUAVOBJ)/eventdispatcher.c
SRC += $(OPUAVOBJ)/uavobjectsinit_linker.c
else
## TESTCODE
SRC += $(OPTESTS)/test_common.c
@ -388,6 +387,9 @@ ifeq ($(DEBUG),YES)
CFLAGS = -g$(DEBUGF) -DDEBUG
endif
# OP has enough memory to always enable optional objects
CFLAGS += -DDIAGNOSTICS
CFLAGS += -O$(OPT)
CFLAGS += -mcpu=$(MCU)
CFLAGS += $(CDEFS)

3
flight/OpenPilot/Makefile.posix
View File

@ -165,6 +165,7 @@ endif
SRC += $(PIOSPOSIX)/pios_crc.c
SRC += $(PIOSPOSIX)/pios_sys.c
SRC += $(PIOSPOSIX)/pios_led.c
SRC += $(PIOSPOSIX)/pios_irq.c
SRC += $(PIOSPOSIX)/pios_delay.c
SRC += $(PIOSPOSIX)/pios_sdcard.c
SRC += $(PIOSPOSIX)/pios_udp.c
@ -176,7 +177,7 @@ SRC += $(PIOSPOSIX)/pios_debug.c
SRC += $(PIOSPOSIX)/pios_rcvr.c
## Libraries for flight calculations
#SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/fifo_buffer.c
SRC += $(FLIGHTLIB)/WorldMagModel.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
## RTOS and RTOS Portable

12
flight/OpenPilot/System/alarms.c
View File

@ -41,14 +41,12 @@ static xSemaphoreHandle lock;
static int32_t hasSeverity(SystemAlarmsAlarmOptions severity);
/**
* Initialize the alarms library
* Initialize the alarms library
*/
int32_t AlarmsInitialize(void)
{
SystemAlarmsInitialize();
lock = xSemaphoreCreateRecursiveMutex();
//do not change the default states of the alarms, let the init code generated by the uavobjectgenerator handle that
//AlarmsClearAll();
//AlarmsDefaultAll();
return 0;
}
@ -56,7 +54,7 @@ int32_t AlarmsInitialize(void)
* Set an alarm
* @param alarm The system alarm to be modified
* @param severity The alarm severity
* @return 0 if success, -1 if an error
* @return 0 if success, -1 if an error
*/
int32_t AlarmsSet(SystemAlarmsAlarmElem alarm, SystemAlarmsAlarmOptions severity)
{
@ -151,7 +149,7 @@ void AlarmsClearAll()
/**
* Check if there are any alarms with the given or higher severity
* @return 0 if no alarms are found, 1 if at least one alarm is found
* @return 0 if no alarms are found, 1 if at least one alarm is found
*/
int32_t AlarmsHasWarnings()
{
@ -208,5 +206,5 @@ static int32_t hasSeverity(SystemAlarmsAlarmOptions severity)
/**
* @}
* @}
*/
*/

8
flight/OpenPilot/System/inc/FreeRTOSConfig.h
View File

@ -39,7 +39,7 @@
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_COUNTING_SEMAPHORES 0
#define configUSE_ALTERNATIVE_API 0
#define configCHECK_FOR_STACK_OVERFLOW 2
//#define configCHECK_FOR_STACK_OVERFLOW 2
#define configQUEUE_REGISTRY_SIZE 10
/* Co-routine definitions. */
@ -72,7 +72,9 @@ NVIC value of 255. */
#define configLIBRARY_KERNEL_INTERRUPT_PRIORITY 15
/* Enable run time stats collection */
#if defined(DEBUG)
#if defined(DIAGNOSTICS)
#define configCHECK_FOR_STACK_OVERFLOW 2
#define configGENERATE_RUN_TIME_STATS 1
#define INCLUDE_uxTaskGetRunTime 1
#define portCONFIGURE_TIMER_FOR_RUN_TIME_STATS()\
@ -81,6 +83,8 @@ do {\
(*(unsigned long *)0xe0001000) |= 1; /* DWT_CTRL |= DWT_CYCCNT_ENA */\
} while(0)
#define portGET_RUN_TIME_COUNTER_VALUE() (*(unsigned long *)0xe0001004)/* DWT_CYCCNT */
#else
#define configCHECK_FOR_STACK_OVERFLOW 1
#endif
#if !defined(ARCH_POSIX) && !defined(ARCH_WIN32)

17
flight/OpenPilot/System/inc/pios_board_posix.h
View File

@ -52,15 +52,22 @@
//-------------------------
//#define PIOS_USART_TX_BUFFER_SIZE 256
#define PIOS_COM_BUFFER_SIZE 1024
#define PIOS_COM_MAX_DEVS 256
#define PIOS_UDP_RX_BUFFER_SIZE PIOS_COM_BUFFER_SIZE
#define PIOS_COM_TELEM_RF 0
#define PIOS_COM_GPS 1
#define PIOS_COM_TELEM_USB 2
extern uint32_t pios_com_telem_rf_id;
extern uint32_t pios_com_telem_usb_id;
extern uint32_t pios_com_gps_id;
extern uint32_t pios_com_aux_id;
extern uint32_t pios_com_spectrum_id;
#define PIOS_COM_TELEM_RF (pios_com_telem_rf_id)
#define PIOS_COM_TELEM_USB (pios_com_telem_usb_id)
#define PIOS_COM_GPS (pios_com_gps_id)
#ifdef PIOS_ENABLE_AUX_UART
#define PIOS_COM_AUX 3
#define PIOS_COM_DEBUG PIOS_COM_AUX
#define PIOS_COM_AUX (pios_com_aux_id)
#define PIOS_COM_DEBUG (PIOS_COM_AUX
#endif
/**

3
flight/OpenPilot/System/inc/pios_config_posix.h
View File

@ -36,6 +36,9 @@
#define PIOS_INCLUDE_SDCARD
#define PIOS_INCLUDE_FREERTOS
#define PIOS_INCLUDE_COM
#define PIOS_INCLUDE_GPS
#define PIOS_INCLUDE_IRQ
#define PIOS_INCLUDE_TELEMETRY_RF
#define PIOS_INCLUDE_UDP
#define PIOS_INCLUDE_SERVO
#define PIOS_INCLUDE_RCVR

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

@ -1023,7 +1023,6 @@ void PIOS_Board_Init(void) {
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
#if defined(PIOS_INCLUDE_RTC)
/* Initialize the real-time clock and its associated tick */

137
flight/OpenPilot/System/pios_board_posix.c
View File

@ -29,6 +29,10 @@
#include <openpilot.h>
#include <uavobjectsinit.h>
#include "attituderaw.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "pios_rcvr_priv.h"
struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS];
@ -40,42 +44,16 @@ void Stack_Change() {
void Stack_Change_Weak() {
}
/**
* PIOS_Board_Init()
* initializes all the core systems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Delay system */
PIOS_DELAY_Init();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
/* Initialize the PiOS library */
PIOS_COM_Init();
}
const struct pios_udp_cfg pios_udp0_cfg = {
const struct pios_udp_cfg pios_udp_telem_cfg = {
.ip = "0.0.0.0",
.port = 9000,
};
const struct pios_udp_cfg pios_udp1_cfg = {
const struct pios_udp_cfg pios_udp_gps_cfg = {
.ip = "0.0.0.0",
.port = 9001,
};
const struct pios_udp_cfg pios_udp2_cfg = {
const struct pios_udp_cfg pios_udp_debug_cfg = {
.ip = "0.0.0.0",
.port = 9002,
};
@ -84,15 +62,20 @@ const struct pios_udp_cfg pios_udp2_cfg = {
/*
* AUX USART
*/
const struct pios_udp_cfg pios_udp3_cfg = {
const struct pios_udp_cfg pios_udp_aux_cfg = {
.ip = "0.0.0.0",
.port = 9003,
};
#endif
#define PIOS_COM_TELEM_RF_RX_BUF_LEN 192
#define PIOS_COM_TELEM_RF_TX_BUF_LEN 192
#define PIOS_COM_GPS_RX_BUF_LEN 192
/*
* Board specific number of devices.
*/
/*
struct pios_udp_dev pios_udp_devs[] = {
#define PIOS_UDP_TELEM 0
{
@ -115,7 +98,7 @@ struct pios_udp_dev pios_udp_devs[] = {
};
uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
*/
/*
* COM devices
*/
@ -126,28 +109,78 @@ uint8_t pios_udp_num_devices = NELEMENTS(pios_udp_devs);
extern const struct pios_com_driver pios_serial_com_driver;
extern const struct pios_com_driver pios_udp_com_driver;
struct pios_com_dev pios_com_devs[] = {
{
.id = PIOS_UDP_TELEM,
.driver = &pios_udp_com_driver,
},
{
.id = PIOS_UDP_GPS,
.driver = &pios_udp_com_driver,
},
{
.id = PIOS_UDP_LOCAL,
.driver = &pios_udp_com_driver,
},
#ifdef PIOS_COM_AUX
{
.id = PIOS_UDP_AUX,
.driver = &pios_udp_com_driver,
},
#endif
};
uint32_t pios_com_telem_rf_id;
uint32_t pios_com_telem_usb_id;
uint32_t pios_com_gps_id;
uint32_t pios_com_aux_id;
uint32_t pios_com_spectrum_id;
const uint8_t pios_com_num_devices = NELEMENTS(pios_com_devs);
/**
* PIOS_Board_Init()
* initializes all the core systems on this specific hardware
* called from System/openpilot.c
*/
void PIOS_Board_Init(void) {
/* Delay system */
PIOS_DELAY_Init();
/* Initialize UAVObject libraries */
EventDispatcherInitialize();
UAVObjInitialize();
UAVObjectsInitializeAll();
/* Initialize the alarms library */
AlarmsInitialize();
/* Initialize the task monitor library */
TaskMonitorInitialize();
#if defined(PIOS_INCLUDE_COM)
#if defined(PIOS_INCLUDE_TELEMETRY_RF)
{
uint32_t pios_udp_telem_rf_id;
if (PIOS_UDP_Init(&pios_udp_telem_rf_id, &pios_udp_telem_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_udp_com_driver, pios_udp_telem_rf_id,
rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN,
tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_TELEMETRY_RF */
#if defined(PIOS_INCLUDE_GPS)
{
uint32_t pios_udp_gps_id;
if (PIOS_UDP_Init(&pios_udp_gps_id, &pios_udp_gps_cfg)) {
PIOS_Assert(0);
}
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN);
PIOS_Assert(rx_buffer);
PIOS_Assert(tx_buffer);
if (PIOS_COM_Init(&pios_com_gps_id, &pios_udp_com_driver, pios_udp_gps_id,
rx_buffer, PIOS_COM_GPS_RX_BUF_LEN,
tx_buffer, PIOS_COM_GPS_RX_BUF_LEN)) {
PIOS_Assert(0);
}
}
#endif /* PIOS_INCLUDE_GPS */
#endif
// Initialize these here as posix has no AHRSComms
AttitudeRawInitialize();
VelocityActualInitialize();
PositionActualInitialize();
}
/**
* @}

5
flight/OpenPilot/System/taskmonitor.c
View File

@ -46,7 +46,10 @@ int32_t TaskMonitorInitialize(void)
{
lock = xSemaphoreCreateRecursiveMutex();
memset(handles, 0, sizeof(xTaskHandle)*TASKINFO_RUNNING_NUMELEM);
lastMonitorTime = 0;
#if defined(DIAGNOSTICS)
lastMonitorTime = portGET_RUN_TIME_COUNTER_VALUE();
#endif
return 0;
}
@ -91,6 +94,7 @@ int32_t TaskMonitorRemove(TaskInfoRunningElem task)
*/
void TaskMonitorUpdateAll(void)
{
#if defined(DIAGNOSTICS)
TaskInfoData data;
int n;
@ -142,4 +146,5 @@ void TaskMonitorUpdateAll(void)
// Done
xSemaphoreGiveRecursive(lock);
#endif
}

2
flight/OpenPilot/UAVObjects.inc
View File

@ -69,6 +69,8 @@ UAVOBJSRCFILENAMES += velocityactual
UAVOBJSRCFILENAMES += velocitydesired
UAVOBJSRCFILENAMES += watchdogstatus
UAVOBJSRCFILENAMES += flightstatus
UAVOBJSRCFILENAMES += cameradesired
UAVOBJSRCFILENAMES += camerastabsettings
UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c )
UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) )

120
flight/PiOS.posix/inc/pios_com.h
View File

@ -1,55 +1,65 @@
/**
******************************************************************************
*
* @file pios_com.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions header
* @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_COM_H
#define PIOS_COM_H
/* Public Functions */
extern int32_t PIOS_COM_Init(void);
extern int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud);
extern int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c);
extern int32_t PIOS_COM_SendChar(uint8_t port, char c);
extern int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str);
extern int32_t PIOS_COM_SendString(uint8_t port, char *str);
extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...);
extern int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...);
extern uint8_t PIOS_COM_ReceiveBuffer(uint8_t port);
extern int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port);
extern int32_t PIOS_COM_ReceiveHandler(void);
struct pios_com_driver {
void (*init)(uint8_t id);
void (*set_baud)(uint8_t id, uint32_t baud);
int32_t (*tx_nb)(uint8_t id, uint8_t *buffer, uint16_t len);
int32_t (*tx)(uint8_t id, uint8_t *buffer, uint16_t len);
int32_t (*rx)(uint8_t id);
int32_t (*rx_avail)(uint8_t id);
};
#endif /* PIOS_COM_H */
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_COM COM layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_com.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions header
* @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_COM_H
#define PIOS_COM_H
typedef uint16_t (*pios_com_callback)(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * task_woken);
struct pios_com_driver {
void (*init)(uint32_t id);
void (*set_baud)(uint32_t id, uint32_t baud);
void (*tx_start)(uint32_t id, uint16_t tx_bytes_avail);
void (*rx_start)(uint32_t id, uint16_t rx_bytes_avail);
void (*bind_rx_cb)(uint32_t id, pios_com_callback rx_in_cb, uint32_t context);
void (*bind_tx_cb)(uint32_t id, pios_com_callback tx_out_cb, uint32_t context);
};
/* Public Functions */
extern int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len);
extern int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud);
extern int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c);
extern int32_t PIOS_COM_SendChar(uint32_t com_id, char c);
extern int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len);
extern int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str);
extern int32_t PIOS_COM_SendString(uint32_t com_id, const char *str);
extern int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...);
extern int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...);
extern uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms);
extern int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id);
#endif /* PIOS_COM_H */
/**
* @}
* @}
*/

17
flight/PiOS.posix/inc/pios_com_priv.h
View File

@ -1,5 +1,10 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_COM COM layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_com_priv.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
@ -29,13 +34,11 @@
#include <pios.h>
struct pios_com_dev {
uint8_t id;
const struct pios_com_driver * const driver;
};
extern struct pios_com_dev pios_com_devs[];
extern const uint8_t pios_com_num_devices;
extern int32_t PIOS_COM_ReceiveHandler(uint32_t com_id);
#endif /* PIOS_COM_PRIV_H */
/**
* @}
* @}
*/

1
flight/PiOS.posix/inc/pios_initcall.h
View File

@ -51,7 +51,6 @@ extern initmodule_t __module_initcall_start[], __module_initcall_end[];
extern void InitModules();
extern void StartModules();
#define UAVOBJ_INITCALL(fn)
#define MODULE_INITCALL(ifn, sfn)
#define MODULE_TASKCREATE_ALL { \

38
flight/PiOS.posix/inc/pios_irq.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_IRQ IRQ Setup Functions
* @{
*
* @file pios_irq.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief IRQ functions header.
* @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_IRQ_H
#define PIOS_IRQ_H
/* Public Functions */
extern int32_t PIOS_IRQ_Disable(void);
extern int32_t PIOS_IRQ_Enable(void);
#endif /* PIOS_IRQ_H */

16
flight/PiOS.posix/inc/pios_udp.h
View File

@ -31,21 +31,5 @@
/* Global Types */
/* Public Functions */
//extern void PIOS_UDP_Init(void);
void PIOS_UDP_Init(void);
extern void PIOS_UDP_ChangeBaud(uint8_t usart, uint32_t baud);
extern int32_t PIOS_UDP_RxBufferFree(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferUsed(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferGet(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferPeek(uint8_t usart);
extern int32_t PIOS_UDP_RxBufferPut(uint8_t usart, uint8_t b);
extern int32_t PIOS_UDP_TxBufferFree(uint8_t usart);
extern int32_t PIOS_UDP_TxBufferGet(uint8_t usart);
extern int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t usart, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_UDP_TxBufferPutMore(uint8_t usart, uint8_t *buffer, uint16_t len);
extern int32_t PIOS_UDP_TxBufferPutNonBlocking(uint8_t usart, uint8_t b);
extern int32_t PIOS_UDP_TxBufferPut(uint8_t usart, uint8_t b);
#endif /* PIOS_UDP_H */

33
flight/PiOS.posix/inc/pios_udp_priv.h
View File

@ -29,6 +29,7 @@
#include <pios.h>
#include <stdio.h>
#include <pthread.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <stdlib.h>
@ -37,30 +38,34 @@
#include <fcntl.h>
#include <netinet/in.h>
struct pios_udp_cfg {
const char * ip;
uint16_t port;
};
struct pios_udp_buffer {
uint8_t buf[PIOS_UDP_RX_BUFFER_SIZE];
uint16_t head;
uint16_t tail;
uint16_t size;
};
typedef struct {
const struct pios_udp_cfg * cfg;
pthread_t rxThread;
struct pios_udp_dev {
const struct pios_udp_cfg * const cfg;
struct pios_udp_buffer rx;
int socket;
struct sockaddr_in server;
struct sockaddr_in client;
uint32_t clientLength;
};
extern struct pios_udp_dev pios_udp_devs[];
extern uint8_t pios_udp_num_devices;
pthread_cond_t cond;
pthread_mutex_t mutex;
pios_com_callback tx_out_cb;
uint32_t tx_out_context;
pios_com_callback rx_in_cb;
uint32_t rx_in_context;
uint8_t rx_buffer[PIOS_UDP_RX_BUFFER_SIZE];
uint8_t tx_buffer[PIOS_UDP_RX_BUFFER_SIZE];
} pios_udp_dev;
extern int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg);
#endif /* PIOS_UDP_PRIV_H */

1
flight/PiOS.posix/pios.h
View File

@ -58,6 +58,7 @@
#include <pios_sys.h>
#include <pios_delay.h>
#include <pios_led.h>
#include <pios_irq.h>
#include <pios_sdcard.h>
#include <pios_udp.h>
#include <pios_com.h>

743
flight/PiOS.posix/posix/pios_com.c
View File

@ -1,285 +1,522 @@
/**
******************************************************************************
*
* @file pios_com.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions
* @see The GNU Public License (GPL) Version 3
* @defgroup PIOS_COM COM layer functions
* @{
*
*****************************************************************************/
/*
* 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
*/
/* Project Includes */
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_COM COM layer functions
* @brief Hardware communication layer
* @{
*
* @file pios_com.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org)
* @brief COM layer functions
* @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
*/
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_COM)
#include "fifo_buffer.h"
#include <pios_com_priv.h>
static struct pios_com_dev * find_com_dev_by_id (uint8_t port)
{
if (port >= pios_com_num_devices) {
/* Undefined COM port for this board (see pios_board.c) */
return NULL;
}
#if !defined(PIOS_INCLUDE_FREERTOS)
#include "pios_delay.h" /* PIOS_DELAY_WaitmS */
#endif
/* Get a handle for the device configuration */
return &(pios_com_devs[port]);
enum pios_com_dev_magic {
PIOS_COM_DEV_MAGIC = 0xaa55aa55,
};
struct pios_com_dev {
enum pios_com_dev_magic magic;
uint32_t lower_id;
const struct pios_com_driver * driver;
#if defined(PIOS_INCLUDE_FREERTOS)
xSemaphoreHandle tx_sem;
xSemaphoreHandle rx_sem;
#endif
bool has_rx;
bool has_tx;
t_fifo_buffer rx;
t_fifo_buffer tx;
};
static bool PIOS_COM_validate(struct pios_com_dev * com_dev)
{
return (com_dev && (com_dev->magic == PIOS_COM_DEV_MAGIC));
}
/**
* Initialises COM layer
* \param[in] mode currently only mode 0 supported
* \return < 0 if initialisation failed
*/
int32_t PIOS_COM_Init(void)
{
int32_t ret = 0;
/* If any COM assignment: */
#if defined(PIOS_INCLUDE_SERIAL)
PIOS_SERIAL_Init();
#endif
#if defined(PIOS_INCLUDE_UDP)
PIOS_UDP_Init();
#endif
return ret;
}
/**
* Change the port speed without re-initializing
* \param[in] port COM port
* \param[in] baud Requested baud rate
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_ChangeBaud(uint8_t port, uint32_t baud)
#if defined(PIOS_INCLUDE_FREERTOS) && 0
// static struct pios_com_dev * PIOS_COM_alloc(void)
//{
// struct pios_com_dev * com_dev;
//
// com_dev = (struct pios_com_dev *)malloc(sizeof(*com_dev));
// if (!com_dev) return (NULL);
//
// com_dev->magic = PIOS_COM_DEV_MAGIC;
// return(com_dev);
//}
#else
static struct pios_com_dev pios_com_devs[PIOS_COM_MAX_DEVS];
static uint8_t pios_com_num_devs;
static uint32_t PIOS_COM_create(void)
{
struct pios_com_dev * com_dev;
struct pios_com_dev * com_dev;
com_dev = find_com_dev_by_id (port);
if (pios_com_num_devs >= PIOS_COM_MAX_DEVS) {
return (PIOS_COM_MAX_DEVS+1);
}
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
com_dev = &pios_com_devs[pios_com_num_devs++];
com_dev->magic = PIOS_COM_DEV_MAGIC;
/* Invoke the driver function if it exists */
if (com_dev->driver->set_baud) {
com_dev->driver->set_baud(com_dev->id, baud);
}
return 0;
return (pios_com_num_devs);
}
/**
* Sends a package over given port
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint8_t port, uint8_t *buffer, uint16_t len)
static struct pios_com_dev * PIOS_COM_find_dev(uint32_t com_dev_id)
{
struct pios_com_dev * com_dev;
if (!com_dev_id) return NULL;
if (com_dev_id>pios_com_num_devs+1) return NULL;
return &pios_com_devs[com_dev_id-1];
}
#endif
com_dev = find_com_dev_by_id (port);
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield);
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield);
static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield);
static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield);
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/**
* Initialises COM layer
* \param[out] handle
* \param[in] driver
* \param[in] id
* \return < 0 if initialisation failed
*/
int32_t PIOS_COM_Init(uint32_t * com_id, const struct pios_com_driver * driver, uint32_t lower_id, uint8_t * rx_buffer, uint16_t rx_buffer_len, uint8_t * tx_buffer, uint16_t tx_buffer_len)
{
PIOS_Assert(com_id);
PIOS_Assert(driver);
/* Invoke the driver function if it exists */
if (com_dev->driver->tx_nb) {
return com_dev->driver->tx_nb(com_dev->id, buffer, len);
}
bool has_rx = (rx_buffer && rx_buffer_len > 0);
bool has_tx = (tx_buffer && tx_buffer_len > 0);
PIOS_Assert(has_rx || has_tx);
PIOS_Assert(driver->bind_tx_cb || !has_tx);
PIOS_Assert(driver->bind_rx_cb || !has_rx);
return 0;
uint32_t com_dev_id;
struct pios_com_dev * com_dev;
com_dev_id = PIOS_COM_create();
com_dev = PIOS_COM_find_dev(com_dev_id);
if (!com_dev) goto out_fail;
com_dev->driver = driver;
com_dev->lower_id = lower_id;
com_dev->has_rx = has_rx;
com_dev->has_tx = has_tx;
if (has_rx) {
fifoBuf_init(&com_dev->rx, rx_buffer, rx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->rx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_rx_cb)(lower_id, PIOS_COM_RxInCallback, com_dev_id);
if (com_dev->driver->rx_start) {
/* Start the receiver */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
}
if (has_tx) {
fifoBuf_init(&com_dev->tx, tx_buffer, tx_buffer_len);
#if defined(PIOS_INCLUDE_FREERTOS)
vSemaphoreCreateBinary(com_dev->tx_sem);
#endif /* PIOS_INCLUDE_FREERTOS */
(com_dev->driver->bind_tx_cb)(lower_id, PIOS_COM_TxOutCallback, com_dev_id);
}
*com_id = com_dev_id;
return(0);
out_fail:
return(-1);
}
/**
* Sends a package over given port
* (blocking function)
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendBuffer(uint8_t port, uint8_t *buffer, uint16_t len)
static void PIOS_COM_UnblockRx(struct pios_com_dev * com_dev, bool * need_yield)
{
struct pios_com_dev * com_dev;
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken;
xSemaphoreGiveFromISR(com_dev->rx_sem, &xHigherPriorityTaskWoken);
com_dev = find_com_dev_by_id (port);
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->tx) {
return com_dev->driver->tx(com_dev->id, buffer, len);
}
return 0;
if (xHigherPriorityTaskWoken != pdFALSE) {
*need_yield = true;
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
/**
* Sends a single character over given port
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendCharNonBlocking(uint8_t port, char c)
{
return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)&c, 1);
}
/**
* Sends a single character over given port
* (blocking function)
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendChar(uint8_t port, char c)
{
return PIOS_COM_SendBuffer(port, (uint8_t *)&c, 1);
}
/**
* Sends a string over given port
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendStringNonBlocking(uint8_t port, char *str)
{
return PIOS_COM_SendBufferNonBlocking(port, (uint8_t *)str, (uint16_t)strlen(str));
}
/**
* Sends a string over given port
* (blocking function)
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendString(uint8_t port, char *str)
{
return PIOS_COM_SendBuffer(port, (uint8_t *)str, strlen(str));
}
/**
* Sends a formatted string (-> printf) over given port
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* 128 characters supported maximum!
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedStringNonBlocking(uint8_t port, char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBufferNonBlocking(port, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Sends a formatted string (-> printf) over given port
* (blocking function)
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedString(uint8_t port, char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBuffer(port, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Transfer bytes from port buffers into another buffer
* \param[in] port COM port
static void PIOS_COM_UnblockTx(struct pios_com_dev * com_dev, bool * need_yield)
{
#if defined(PIOS_INCLUDE_FREERTOS)
static signed portBASE_TYPE xHigherPriorityTaskWoken;
xSemaphoreGiveFromISR(com_dev->tx_sem, &xHigherPriorityTaskWoken);
if (xHigherPriorityTaskWoken != pdFALSE) {
*need_yield = true;
} else {
*need_yield = false;
}
#else
*need_yield = false;
#endif
}
static uint16_t PIOS_COM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(context);
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(com_dev->has_rx);
PIOS_IRQ_Disable();
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->rx, buf, buf_len);
PIOS_IRQ_Enable();
if (bytes_into_fifo > 0) {
/* Data has been added to the buffer */
PIOS_COM_UnblockRx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getFree(&com_dev->rx);
}
return (bytes_into_fifo);
}
static uint16_t PIOS_COM_TxOutCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(context);
bool valid = PIOS_COM_validate(com_dev);
PIOS_Assert(valid);
PIOS_Assert(buf);
PIOS_Assert(buf_len);
PIOS_Assert(com_dev->has_tx);
PIOS_IRQ_Disable();
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->tx, buf, buf_len);
PIOS_IRQ_Enable();
if (bytes_from_fifo > 0) {
/* More space has been made in the buffer */
PIOS_COM_UnblockTx(com_dev, need_yield);
}
if (headroom) {
*headroom = fifoBuf_getUsed(&com_dev->tx);
}
return (bytes_from_fifo);
}
/**
* Change the port speed without re-initializing
* \param[in] port COM port
* \param[in] baud Requested baud rate
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_ChangeBaud(uint32_t com_id, uint32_t baud)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
/* Invoke the driver function if it exists */
if (com_dev->driver->set_baud) {
com_dev->driver->set_baud(com_dev->lower_id, baud);
}
return 0;
}
/**
* Sends a package over given port
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendBufferNonBlocking(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
if (len >= fifoBuf_getFree(&com_dev->tx)) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
PIOS_IRQ_Disable();
uint16_t bytes_into_fifo = fifoBuf_putData(&com_dev->tx, buffer, len);
PIOS_IRQ_Enable();
if (bytes_into_fifo > 0) {
/* More data has been put in the tx buffer, make sure the tx is started */
if (com_dev->driver->tx_start) {
com_dev->driver->tx_start(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
}
return (0);
}
/**
* Sends a package over given port
* (blocking function)
* \param[in] port COM port
* \param[in] buffer character buffer
* \param[in] len buffer length
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendBuffer(uint32_t com_id, const uint8_t *buffer, uint16_t len)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
return -1;
}
PIOS_Assert(com_dev->has_tx);
int32_t rc;
do {
rc = PIOS_COM_SendBufferNonBlocking(com_id, buffer, len);
#if defined(PIOS_INCLUDE_FREERTOS)
if (rc == -2) {
/* Make sure the transmitter is running while we wait */
if (com_dev->driver->tx_start) {
(com_dev->driver->tx_start)(com_dev->lower_id,
fifoBuf_getUsed(&com_dev->tx));
}
if (xSemaphoreTake(com_dev->tx_sem, portMAX_DELAY) != pdTRUE) {
return -3;
}
}
#endif
} while (rc == -2);
return rc;
}
/**
* Sends a single character over given port
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendCharNonBlocking(uint32_t com_id, char c)
{
return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)&c, 1);
}
/**
* Sends a single character over given port
* (blocking function)
* \param[in] port COM port
* \param[in] c character
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendChar(uint32_t com_id, char c)
{
return PIOS_COM_SendBuffer(com_id, (uint8_t *)&c, 1);
}
/**
* Sends a string over given port
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return -2 buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendStringNonBlocking(uint32_t com_id, const char *str)
{
return PIOS_COM_SendBufferNonBlocking(com_id, (uint8_t *)str, (uint16_t)strlen(str));
}
/**
* Sends a string over given port
* (blocking function)
* \param[in] port COM port
* \param[in] str zero-terminated string
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendString(uint32_t com_id, const char *str)
{
return PIOS_COM_SendBuffer(com_id, (uint8_t *)str, strlen(str));
}
/**
* Sends a formatted string (-> printf) over given port
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* 128 characters supported maximum!
* \return -2 if non-blocking mode activated: buffer is full
* caller should retry until buffer is free again
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedStringNonBlocking(uint32_t com_id, const char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBufferNonBlocking(com_id, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Sends a formatted string (-> printf) over given port
* (blocking function)
* \param[in] port COM port
* \param[in] *format zero-terminated format string - 128 characters supported maximum!
* \param[in] ... optional arguments,
* \return -1 if port not available
* \return 0 on success
*/
int32_t PIOS_COM_SendFormattedString(uint32_t com_id, const char *format, ...)
{
uint8_t buffer[128]; // TODO: tmp!!! Provide a streamed COM method later!
va_list args;
va_start(args, format);
vsprintf((char *)buffer, format, args);
return PIOS_COM_SendBuffer(com_id, buffer, (uint16_t)strlen((char *)buffer));
}
/**
* Transfer bytes from port buffers into another buffer
* \param[in] port COM port
* \returns Byte from buffer
*/
uint8_t PIOS_COM_ReceiveBuffer(uint8_t port)
{
struct pios_com_dev * com_dev;
com_dev = find_com_dev_by_id (port);
//PIOS_DEBUG_Assert(com_dev);
//PIOS_DEBUG_Assert(com_dev->driver->rx);
return com_dev->driver->rx(com_dev->id);
}
/**
* Get the number of bytes waiting in the buffer
* \param[in] port COM port
* \return Number of bytes used in buffer
*/
int32_t PIOS_COM_ReceiveBufferUsed(uint8_t port)
*/
uint16_t PIOS_COM_ReceiveBuffer(uint32_t com_id, uint8_t * buf, uint16_t buf_len, uint32_t timeout_ms)
{
struct pios_com_dev * com_dev;
PIOS_Assert(buf);
PIOS_Assert(buf_len);
com_dev = find_com_dev_by_id (port);
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!com_dev) {
/* Undefined COM port for this board (see pios_board.c) */
return 0;
}
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
PIOS_Assert(0);
}
PIOS_Assert(com_dev->has_rx);
if (!com_dev->driver->rx_avail) {
return 0;
}
check_again:
PIOS_IRQ_Disable();
uint16_t bytes_from_fifo = fifoBuf_getData(&com_dev->rx, buf, buf_len);
PIOS_IRQ_Enable();
return com_dev->driver->rx_avail(com_dev->id);
if (bytes_from_fifo == 0 && timeout_ms > 0) {
/* No more bytes in receive buffer */
/* Make sure the receiver is running while we wait */
if (com_dev->driver->rx_start) {
/* Notify the lower layer that there is now room in the rx buffer */
(com_dev->driver->rx_start)(com_dev->lower_id,
fifoBuf_getFree(&com_dev->rx));
}
#if defined(PIOS_INCLUDE_FREERTOS)
if (xSemaphoreTake(com_dev->rx_sem, timeout_ms / portTICK_RATE_MS) == pdTRUE) {
/* Make sure we don't come back here again */
timeout_ms = 0;
goto check_again;
}
#else
PIOS_DELAY_WaitmS(1);
timeout_ms--;
goto check_again;
#endif
}
/* Return received byte */
return (bytes_from_fifo);
}
#endif
/**
* Get the number of bytes waiting in the buffer
* \param[in] port COM port
* \return Number of bytes used in buffer
*/
int32_t PIOS_COM_ReceiveBufferUsed(uint32_t com_id)
{
struct pios_com_dev * com_dev = PIOS_COM_find_dev(com_id);
if (!PIOS_COM_validate(com_dev)) {
/* Undefined COM port for this board (see pios_board.c) */
PIOS_Assert(0);
}
PIOS_Assert(com_dev->has_rx);
return (fifoBuf_getUsed(&com_dev->rx));
}
#endif
/**
* @}
* @}
*/

69
flight/PiOS.posix/posix/pios_irq.c Normal file
View File

@ -0,0 +1,69 @@
/**
******************************************************************************
* @addtogroup PIOS PIOS Core hardware abstraction layer
* @{
* @addtogroup PIOS_IRQ IRQ Setup Functions
* @brief STM32 Hardware code to enable and disable interrupts
* @{
*
* @file pios_irq.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* Parts by Thorsten Klose (tk@midibox.org) (tk@midibox.org)
* @brief IRQ Enable/Disable routines
* @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
*/
/* Project Includes */
#include "pios.h"
#if defined(PIOS_INCLUDE_IRQ)
/* Private Function Prototypes */
/**
* Disables all interrupts (nested)
* \return < 0 On errors
*/
int32_t PIOS_IRQ_Disable(void)
{
#if defined(PIOS_INCLUDE_FREERTOS)
taskENTER_CRITICAL();
#endif
return 0;
}
/**
* Enables all interrupts (nested)
* \return < 0 on errors
* \return -1 on nesting errors (PIOS_IRQ_Disable() hasn't been called before)
*/
int32_t PIOS_IRQ_Enable(void)
{
#if defined(PIOS_INCLUDE_FREERTOS)
taskEXIT_CRITICAL();
#endif
return 0;
}
#endif
/**
* @}
* @}
*/

536
flight/PiOS.posix/posix/pios_udp.c
View File

@ -32,429 +32,213 @@
#if defined(PIOS_INCLUDE_UDP)
#include <signal.h>
#include <pios_udp_priv.h>
/* We need a list of UDP devices */
#define PIOS_UDP_MAX_DEV 256
static int8_t pios_udp_num_devices = 0;
static pios_udp_dev pios_udp_devices[PIOS_UDP_MAX_DEV];
/* Provide a COM driver */
static void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud);
static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context);
static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context);
static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail);
static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail);
const struct pios_com_driver pios_udp_com_driver = {
.set_baud = PIOS_UDP_ChangeBaud,
.tx_nb = PIOS_UDP_TxBufferPutMoreNonBlocking,
.tx = PIOS_UDP_TxBufferPutMore,
.rx = PIOS_UDP_RxBufferGet,
.rx_avail = PIOS_UDP_RxBufferUsed,
.set_baud = PIOS_UDP_ChangeBaud,
.tx_start = PIOS_UDP_TxStart,
.rx_start = PIOS_UDP_RxStart,
.bind_tx_cb = PIOS_UDP_RegisterTxCallback,
.bind_rx_cb = PIOS_UDP_RegisterRxCallback,
};
static struct pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
static pios_udp_dev * find_udp_dev_by_id (uint8_t udp)
{
if (udp >= pios_udp_num_devices) {
/* Undefined UDP port for this board (see pios_board.c) */
PIOS_Assert(0);
return NULL;
}
/* Get a handle for the device configuration */
return &(pios_udp_devs[udp]);
return &(pios_udp_devices[udp]);
}
/**
* Open some UDP sockets
*/
void PIOS_UDP_Init(void)
{
struct pios_udp_dev * udp_dev;
uint8_t i;
for (i = 0; i < pios_udp_num_devices; i++) {
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(i);
//PIOS_DEBUG_Assert(udp_dev);
/* Clear buffer counters */
udp_dev->rx.head = udp_dev->rx.tail = udp_dev->rx.size = 0;
/* assign socket */
udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
memset(&udp_dev->server,0,sizeof(udp_dev->server));
memset(&udp_dev->client,0,sizeof(udp_dev->client));
udp_dev->server.sin_family = AF_INET;
udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip);
udp_dev->server.sin_port = htons(udp_dev->cfg->port);
int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server));
/* use nonblocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK);
printf("udp dev %i - socket %i opened - result %i\n",i,udp_dev->socket,res);
/* TODO do some error handling - wait no, we can't - we are void anyway ;) */
}
}
/**
* Changes the baud rate of the UDP peripheral without re-initialising.
* \param[in] udp UDP name (GPS, TELEM, AUX)
* \param[in] baud Requested baud rate
*/
void PIOS_UDP_ChangeBaud(uint8_t udp, uint32_t baud)
{
}
/**
* puts a byte onto the receive buffer
* \param[in] UDP UDP name
* \param[in] b byte which should be put into Rx buffer
* \return 0 if no error
* \return -1 if UDP not available
* \return -2 if buffer full (retry)
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferPut(uint8_t udp, uint8_t b)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -1;
}
if (udp_dev->rx.size >= sizeof(udp_dev->rx.buf)) {
/* Buffer full (retry) */
return -2;
}
/* Copy received byte into receive buffer */
udp_dev->rx.buf[udp_dev->rx.head++] = b;
if (udp_dev->rx.head >= sizeof(udp_dev->rx.buf)) {
udp_dev->rx.head = 0;
}
udp_dev->rx.size++;
/* No error */
return 0;
}
/**
* attempt to receive
* RxThread
*/
void PIOS_UDP_RECV(uint8_t udp) {
struct pios_udp_dev * udp_dev;
unsigned char localbuffer[PIOS_UDP_RX_BUFFER_SIZE];
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return;
}
/* use nonblocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK);
/* receive data */
int received;
udp_dev->clientLength=sizeof(udp_dev->client);
if ((received = recvfrom(udp_dev->socket,
localbuffer,
(PIOS_UDP_RX_BUFFER_SIZE - udp_dev->rx.size),
0,
(struct sockaddr *) &udp_dev->client,
(socklen_t*)&udp_dev->clientLength)) < 0) {
return;
}
/* copy received data to buffer */
int t;
for (t=0;t<received;t++) {
PIOS_UDP_RxBufferPut(udp,localbuffer[t]);
}
}
/**
* Returns number of free bytes in receive buffer
* \param[in] UDP UDP name
* \return UDP number of free bytes
* \return 1: UDP available
* \return 0: UDP not available
*/
int32_t PIOS_UDP_RxBufferFree(uint8_t udp)
void * PIOS_UDP_RxThread(void * udp_dev_n)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
/* needed because of FreeRTOS.posix scheduling */
sigset_t set;
sigfillset(&set);
sigprocmask(SIG_BLOCK, &set, NULL);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
pios_udp_dev * udp_dev = (pios_udp_dev*) udp_dev_n;
/* fill buffer */
PIOS_UDP_RECV(udp);
/**
* com devices never get closed except by application "reboot"
* we also never give up our mutex except for waiting
*/
while(1) {
return (sizeof(udp_dev->rx.buf) - udp_dev->rx.size);
}
/**
* receive
*/
int received;
udp_dev->clientLength=sizeof(udp_dev->client);
if ((received = recvfrom(udp_dev->socket,
&udp_dev->rx_buffer,
PIOS_UDP_RX_BUFFER_SIZE,
0,
(struct sockaddr *) &udp_dev->client,
(socklen_t*)&udp_dev->clientLength)) >= 0)
{
/**
* Returns number of used bytes in receive buffer
* \param[in] UDP UDP name
* \return > 0: number of used bytes
* \return 0 if UDP not available
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferUsed(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* copy received data to buffer if possible */
/* we do NOT buffer data locally. If the com buffer can't receive, data is discarded! */
/* (thats what the USART driver does too!) */
bool rx_need_yield = false;
if (udp_dev->rx_in_cb) {
(void) (udp_dev->rx_in_cb)(udp_dev->rx_in_context, udp_dev->rx_buffer, received, NULL, &rx_need_yield);
}
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
#if defined(PIOS_INCLUDE_FREERTOS)
if (rx_need_yield) {
vPortYieldFromISR();
}
#endif /* PIOS_INCLUDE_FREERTOS */
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
}
/* fill buffer */
PIOS_UDP_RECV(udp);
return (udp_dev->rx.size);
}
/**
* Gets a byte from the receive buffer
* \param[in] UDP UDP name
* \return -1 if UDP not available
* \return -2 if no new byte available
* \return >= 0: number of received bytes
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferGet(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
/* fill buffer */
PIOS_UDP_RECV(udp);
if (!udp_dev->rx.size) {
/* Nothing new in the buffer */
return -1;
}
/* get byte */
uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail++];
if (udp_dev->rx.tail >= sizeof(udp_dev->rx.buf)) {
udp_dev->rx.tail = 0;
}
udp_dev->rx.size--;
/* Return received byte */
return b;
}
/**
* Returns the next byte of the receive buffer without taking it
* \param[in] UDP UDP name
* \return -1 if UDP not available
* \return -2 if no new byte available
* \return >= 0: number of received bytes
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_RxBufferPeek(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -2;
}
/* fill buffer */
PIOS_UDP_RECV(udp);
if (!udp_dev->rx.size) {
/* Nothing new in the buffer */
return -1;
}
/* get byte */
uint8_t b = udp_dev->rx.buf[udp_dev->rx.tail];
/* Return received byte */
return b;
}
/**
* returns number of free bytes in transmit buffer
* \param[in] UDP UDP name
* \return number of free bytes
* \return 0 if UDP not available
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferFree(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return 0;
}
return PIOS_UDP_RX_BUFFER_SIZE;
}
/**
* returns number of used bytes in transmit buffer
* \param[in] UDP UDP name
* \return number of used bytes
* \return 0 if UDP not available
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferUsed(uint8_t udp)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return 0;
}
return 0;
}
}
/**
* puts more than one byte onto the transmit buffer (used for atomic sends)
* \param[in] UDP UDP name
* \param[in] *buffer pointer to buffer to be sent
* \param[in] len number of bytes to be sent
* \return 0 if no error
* \return -1 if UDP not available
* \return -2 if buffer full or cannot get all requested bytes (retry)
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
* Open UDP socket
*/
int32_t PIOS_UDP_TxBufferPutMoreNonBlocking(uint8_t udp, uint8_t *buffer, uint16_t len)
int32_t PIOS_UDP_Init(uint32_t * udp_id, const struct pios_udp_cfg * cfg)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
pios_udp_dev * udp_dev = &pios_udp_devices[pios_udp_num_devices];
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -1;
}
if (len >= PIOS_UDP_RX_BUFFER_SIZE) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
/* send data to client - non blocking*/
/* use nonblocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags | O_NONBLOCK);
sendto(udp_dev->socket, buffer, len, 0,
(struct sockaddr *) &udp_dev->client,
(socklen_t)sizeof(udp_dev->client));
pios_udp_num_devices++;
/* No error */
return 0;
/* initialize */
udp_dev->rx_in_cb = NULL;
udp_dev->tx_out_cb = NULL;
udp_dev->cfg=cfg;
/* assign socket */
udp_dev->socket = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
memset(&udp_dev->server,0,sizeof(udp_dev->server));
memset(&udp_dev->client,0,sizeof(udp_dev->client));
udp_dev->server.sin_family = AF_INET;
udp_dev->server.sin_addr.s_addr = inet_addr(udp_dev->cfg->ip);
udp_dev->server.sin_port = htons(udp_dev->cfg->port);
int res= bind(udp_dev->socket, (struct sockaddr *)&udp_dev->server,sizeof(udp_dev->server));
/* Create transmit thread for this connection */
pthread_create(&udp_dev->rxThread, NULL, PIOS_UDP_RxThread, (void*)udp_dev);
printf("udp dev %i - socket %i opened - result %i\n",pios_udp_num_devices-1,udp_dev->socket,res);
*udp_id = pios_udp_num_devices-1;
return res;
}
/**
* puts more than one byte onto the transmit buffer (used for atomic sends)<BR>
* (blocking function)
* \param[in] UDP UDP name
* \param[in] *buffer pointer to buffer to be sent
* \param[in] len number of bytes to be sent
* \return 0 if no error
* \return -1 if UDP not available
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferPutMore(uint8_t udp, uint8_t *buffer, uint16_t len)
void PIOS_UDP_ChangeBaud(uint32_t udp_id, uint32_t baud)
{
struct pios_udp_dev * udp_dev;
/* Get a handle for the device configuration */
udp_dev = find_udp_dev_by_id(udp);
if (!udp_dev) {
/* Undefined UDP port for this board (see pios_board.c) */
return -1;
}
if (len >= PIOS_UDP_RX_BUFFER_SIZE) {
/* Buffer cannot accept all requested bytes (retry) */
return -2;
}
/* send data to client - blocking*/
/* use blocking IO */
int flags = fcntl(udp_dev->socket, F_GETFL, 0);
fcntl(udp_dev->socket, F_SETFL, flags & ~O_NONBLOCK);
sendto(udp_dev->socket, buffer, len, 0,
(struct sockaddr *) &udp_dev->client,
sizeof(udp_dev->client));
/* No error */
return 0;
/**
* doesn't apply!
*/
}
/**
* puts a byte onto the transmit buffer
* \param[in] UDP UDP name
* \param[in] b byte which should be put into Tx buffer
* \return 0 if no error
* \return -1 if UDP not available
* \return -2 if buffer full (retry)
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferPut_NonBlocking(uint8_t udp, uint8_t b)
static void PIOS_UDP_RxStart(uint32_t udp_id, uint16_t rx_bytes_avail)
{
return PIOS_UDP_TxBufferPutMoreNonBlocking(udp, &b, 1);
/**
* lazy!
*/
}
/**
* puts a byte onto the transmit buffer<BR>
* (blocking function)
* \param[in] UDP UDP name
* \param[in] b byte which should be put into Tx buffer
* \return 0 if no error
* \return -1 if UDP not available
* \return -3 if UDP not supported by UDPTxBufferPut Routine
* \note Applications shouldn't call these functions directly, instead please use \ref PIOS_COM layer functions
*/
int32_t PIOS_UDP_TxBufferPut(uint8_t udp, uint8_t b)
static void PIOS_UDP_TxStart(uint32_t udp_id, uint16_t tx_bytes_avail)
{
return PIOS_UDP_TxBufferPutMore(udp, &b, 1);
pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);
PIOS_Assert(udp_dev);
int32_t length,len,rem;
/**
* we send everything directly whenever notified of data to send (lazy!)
*/
if (udp_dev->tx_out_cb) {
while (tx_bytes_avail>0) {
bool tx_need_yield = false;
length = (udp_dev->tx_out_cb)(udp_dev->tx_out_context, udp_dev->tx_buffer, PIOS_UDP_RX_BUFFER_SIZE, NULL, &tx_need_yield);
rem = length;
while (rem>0) {
len = sendto(udp_dev->socket, udp_dev->tx_buffer, length, 0,
(struct sockaddr *) &udp_dev->client,
sizeof(udp_dev->client));
if (len<=0) {
rem=0;
} else {
rem -= len;
}
}
tx_bytes_avail -= length;
}
}
}
static void PIOS_UDP_RegisterRxCallback(uint32_t udp_id, pios_com_callback rx_in_cb, uint32_t context)
{
pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);
PIOS_Assert(udp_dev);
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
udp_dev->rx_in_context = context;
udp_dev->rx_in_cb = rx_in_cb;
}
static void PIOS_UDP_RegisterTxCallback(uint32_t udp_id, pios_com_callback tx_out_cb, uint32_t context)
{
pios_udp_dev * udp_dev = find_udp_dev_by_id(udp_id);
PIOS_Assert(udp_dev);
/*
* Order is important in these assignments since ISR uses _cb
* field to determine if it's ok to dereference _cb and _context
*/
udp_dev->tx_out_context = context;
udp_dev->tx_out_cb = tx_out_cb;
}
#endif

1
flight/PiOS.win32/inc/pios_initcall.h
View File

@ -38,7 +38,6 @@
* and we cannot define a linker script for each of them atm
*/
#define UAVOBJ_INITCALL(fn)
#define MODULE_INITCALL(ifn, iparam, sfn, sparam, flags)
#define MODULE_TASKCREATE_ALL

16
flight/PiOS/Common/pios_adxl345.c
View File

@ -96,6 +96,22 @@ void PIOS_ADXL345_Init()
PIOS_ADXL345_SetMeasure(1);
}
/**
* @brief Return number of entries in the fifo
*/
uint8_t PIOS_ADXL345_FifoElements()
{
uint8_t buf[2] = {0,0};
uint8_t rec[2] = {0,0};
buf[0] = ADXL_FIFOSTATUS_ADDR | ADXL_READ_BIT ; // Read fifo status
PIOS_ADXL345_ClaimBus();
PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,&buf[0],&rec[0],sizeof(buf),NULL);
PIOS_ADXL345_ReleaseBus();
return rec[1] & 0x3f;
}
/**
* @brief Read a single set of values from the x y z channels
* @returns The number of samples remaining in the fifo

10
flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_BL_sections.ld
View File

@ -18,16 +18,6 @@ SECTIONS
*(.rodata .rodata* .gnu.linkonce.r.*)
} > BL_FLASH
/* init sections */
.initcalluavobj.init :
{
. = ALIGN(4);
__uavobj_initcall_start = .;
KEEP(*(.initcalluavobj.init))
. = ALIGN(4);
__uavobj_initcall_end = .;
} > BL_FLASH
.ARM.extab :
{
*(.ARM.extab* .gnu.linkonce.armextab.*)

10
flight/PiOS/STM32F10x/link_STM32103CB_CC_Rev1_sections.ld
View File

@ -22,16 +22,6 @@ SECTIONS
*(.rodata .rodata* .gnu.linkonce.r.*)
} > FLASH
/* init sections */
.initcalluavobj.init :
{
. = ALIGN(4);
__uavobj_initcall_start = .;
KEEP(*(.initcalluavobj.init))
. = ALIGN(4);
__uavobj_initcall_end = .;
} >FLASH
/* module sections */
.initcallmodule.init :
{

10
flight/PiOS/STM32F10x/link_STM3210E_INS_BL_sections.ld
View File

@ -180,16 +180,6 @@ SECTIONS
} > BL_FLASH
/* init sections */
.initcalluavobj.init :
{
. = ALIGN(4);
__uavobj_initcall_start = .;
KEEP(*(.initcalluavobj.init))
. = ALIGN(4);
__uavobj_initcall_end = .;
} > BL_FLASH
/* the program code is stored in the .text section, which goes to Flash */
.text :
{

11
flight/PiOS/STM32F10x/link_STM3210E_INS_sections.ld
View File

@ -181,16 +181,7 @@ SECTIONS
. = ALIGN(4);
} >FLASH
/* init sections */
.initcalluavobj.init :
{
. = ALIGN(4);
__uavobj_initcall_start = .;
KEEP(*(.initcalluavobj.init))
. = ALIGN(4);
__uavobj_initcall_end = .;
} >FLASH
/* the program code is stored in the .text section, which goes to Flash */
.text :

11
flight/PiOS/STM32F10x/link_STM3210E_OP_BL_sections.ld
View File

@ -179,17 +179,6 @@ SECTIONS
. = ALIGN(4);
} > BL_FLASH
/* init sections */
.initcalluavobj.init :
{
. = ALIGN(4);
__uavobj_initcall_start = .;
KEEP(*(.initcalluavobj.init))
. = ALIGN(4);
__uavobj_initcall_end = .;
} > BL_FLASH
/* the program code is stored in the .text section, which goes to Flash */
.text :
{

11
flight/PiOS/STM32F10x/link_STM3210E_OP_sections.ld
View File

@ -181,17 +181,6 @@ SECTIONS
*(.flashtext) /* Startup code */
. = ALIGN(4);
} >FLASH
/* init sections */
.initcalluavobj.init :
{
. = ALIGN(4);
__uavobj_initcall_start = .;
KEEP(*(.initcalluavobj.init))
. = ALIGN(4);
__uavobj_initcall_end = .;
} >FLASH
/* module sections */
.initcallmodule.init :

2
flight/PiOS/inc/pios_adxl345.h
View File

@ -16,6 +16,7 @@
#define ADXL_MULTI_BIT 0x40
#define ADXL_X0_ADDR 0x32
#define ADXL_FIFOSTATUS_ADDR 0x39
#define ADXL_RATE_ADDR 0x2C
#define ADXL_RATE_100 0x0A
@ -52,5 +53,6 @@ void PIOS_ADXL345_FifoDepth(uint8_t depth);
void PIOS_ADXL345_Attach(uint32_t spi_id);
void PIOS_ADXL345_Init();
uint8_t PIOS_ADXL345_Read(struct pios_adxl345_data * data);
uint8_t PIOS_ADXL345_FifoElements();
#endif

1
flight/PiOS/inc/pios_initcall.h
View File

@ -67,7 +67,6 @@ extern initmodule_t __module_initcall_start[], __module_initcall_end[];
static initmodule_t __initcall_##fn __attribute__((__used__)) \
__attribute__((__section__(".initcall" level ".init"))) = { .fn_minit = ifn, .fn_tinit = sfn };
#define UAVOBJ_INITCALL(fn) __define_initcall("uavobj",fn,1)
#define MODULE_INITCALL(ifn, sfn) __define_module_initcall("module", ifn, sfn)
#define MODULE_INITIALISE_ALL { for (initmodule_t *fn = __module_initcall_start; fn < __module_initcall_end; fn++) \

4
flight/UAVObjects/eventdispatcher.c
View File

@ -27,7 +27,11 @@
#include "openpilot.h"
// Private constants
#if defined(PIOS_EVENTDISAPTCHER_QUEUE)
#define MAX_QUEUE_SIZE PIOS_EVENTDISAPTCHER_QUEUE
#else
#define MAX_QUEUE_SIZE 20
#endif
#if defined(PIOS_EVENTDISPATCHER_STACK_SIZE)
#define STACK_SIZE PIOS_EVENTDISPATCHER_STACK_SIZE

2
flight/UAVObjects/inc/uavobjectsinit.h → flight/UAVObjects/inc/uavobjectsinittemplate.h
View File

@ -29,4 +29,6 @@
void UAVObjectsInitializeAll();
#define UAVOBJECTS_LARGEST $(SIZECALCULATION)
#endif // UAVOBJECTSINIT_H

2
flight/UAVObjects/uavobjectsinittemplate.c
View File

@ -36,5 +36,7 @@ $(OBJINC)
*/
void UAVObjectsInitializeAll()
{
return;
// This function is no longer used anyway
$(OBJINIT)
}

10
flight/UAVObjects/uavobjecttemplate.c
View File

@ -40,15 +40,19 @@
#include "$(NAMELC).h"
// Private variables
static UAVObjHandle handle;
static UAVObjHandle handle = NULL;
/**
* Initialize object.
* \return 0 Success
* \return -1 Failure
* \return -1 Failure to initialize or -2 for already initialized
*/
int32_t $(NAME)Initialize(void)
{
// Don't set the handle to null if already registered
if(UAVObjGetByID($(NAMEUC)_OBJID) != NULL)
return -2;
// Register object with the object manager
handle = UAVObjRegister($(NAMEUC)_OBJID, $(NAMEUC)_NAME, $(NAMEUC)_METANAME, 0,
$(NAMEUC)_ISSINGLEINST, $(NAMEUC)_ISSETTINGS, $(NAMEUC)_NUMBYTES, &$(NAME)SetDefaults);
@ -64,8 +68,6 @@ int32_t $(NAME)Initialize(void)
}
}
UAVOBJ_INITCALL($(NAME)Initialize);
/**
* Initialize object fields and metadata with the default values.
* If a default value is not specified the object fields

21
flight/UAVTalk/inc/uavtalk.h
View File

@ -29,10 +29,6 @@
#ifndef UAVTALK_H
#define UAVTALK_H
// Public constants
#define UAVTALK_WAITFOREVER -1
#define UAVTALK_NOWAIT 0
// Public types
typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length);
@ -47,14 +43,17 @@ typedef struct {
uint32_t rxErrors;
} UAVTalkStats;
typedef void* UAVTalkConnection;
// Public functions
int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream);
int32_t UAVTalkSetOutputStream(UAVTalkOutputStream outputStream);
int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
int32_t UAVTalkProcessInputStream(uint8_t rxbyte);
void UAVTalkGetStats(UAVTalkStats* stats);
void UAVTalkResetStats();
UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream, uint32_t maxPacketSize);
int32_t UAVTalkSetOutputStream(UAVTalkConnection connection, UAVTalkOutputStream outputStream);
UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
int32_t UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats);
void UAVTalkResetStats(UAVTalkConnection connection);
#endif // UAVTALK_H
/**

111
flight/UAVTalk/inc/uavtalk_priv.h Normal file
View File

@ -0,0 +1,111 @@
/**
******************************************************************************
* @addtogroup OpenPilotSystem OpenPilot System
* @{
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
* @{
* @file uavtalk.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Private include file of the UAVTalk library
* @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 UAVTALK_PRIV_H
#define UAVTALK_PRIV_H
#include "uavobjectsinit.h"
// Private types and constants
typedef struct {
uint8_t sync;
uint8_t type;
uint16_t size;
uint32_t objId;
} uavtalk_min_header;
#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header)
typedef struct {
uint8_t sync;
uint8_t type;
uint16_t size;
uint32_t objId;
uint16_t instId;
} uavtalk_max_header;
#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header)
typedef uint8_t uavtalk_checksum;
#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum)
#define UAVTALK_MAX_PAYLOAD_LENGTH UAVOBJECTS_LARGEST
#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH
#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH
typedef enum {UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS} UAVTalkRxState;
typedef struct {
UAVObjHandle obj;
uint8_t type;
uint16_t packet_size;
uint32_t objId;
uint16_t instId;
uint32_t length;
uint8_t cs;
int32_t rxCount;
UAVTalkRxState state;
uint16_t rxPacketLength;
} UAVTalkInputProcessor;
typedef struct {
uint8_t canari;
UAVTalkOutputStream outStream;
xSemaphoreHandle lock;
xSemaphoreHandle transLock;
xSemaphoreHandle respSema;
UAVObjHandle respObj;
uint16_t respInstId;
UAVTalkStats stats;
UAVTalkInputProcessor iproc;
uint8_t *rxBuffer;
uint32_t txSize;
uint8_t *txBuffer;
} UAVTalkConnectionData;
#define UAVTALK_CANARI 0xCA
#define UAVTALK_WAITFOREVER -1
#define UAVTALK_NOWAIT 0
#define UAVTALK_SYNC_VAL 0x3C
#define UAVTALK_TYPE_MASK 0xF8
#define UAVTALK_TYPE_VER 0x20
#define UAVTALK_TYPE_OBJ (UAVTALK_TYPE_VER | 0x00)
#define UAVTALK_TYPE_OBJ_REQ (UAVTALK_TYPE_VER | 0x01)
#define UAVTALK_TYPE_OBJ_ACK (UAVTALK_TYPE_VER | 0x02)
#define UAVTALK_TYPE_ACK (UAVTALK_TYPE_VER | 0x03)
#define UAVTALK_TYPE_NACK (UAVTALK_TYPE_VER | 0x04)
//macros
#define CHECKCONHANDLE(handle,variable,failcommand) \
variable = (UAVTalkConnectionData*) handle; \
if (variable == NULL || variable->canari != UAVTALK_CANARI) { \
failcommand; \
}
#endif // UAVTALK__PRIV_H
/**
* @}
* @}
*/

518
flight/UAVTalk/uavtalk.c
View File

@ -30,113 +30,145 @@
*/
#include "openpilot.h"
#include "uavtalk_priv.h"
// Private constants
#define SYNC_VAL 0x3C
#define TYPE_MASK 0xF8
#define TYPE_VER 0x20
#define TYPE_OBJ (TYPE_VER | 0x00)
#define TYPE_OBJ_REQ (TYPE_VER | 0x01)
#define TYPE_OBJ_ACK (TYPE_VER | 0x02)
#define TYPE_ACK (TYPE_VER | 0x03)
#define TYPE_NACK (TYPE_VER | 0x04)
#define MIN_HEADER_LENGTH 8 // sync(1), type (1), size (2), object ID (4)
#define MAX_HEADER_LENGTH 10 // sync(1), type (1), size (2), object ID (4), instance ID (2, not used in single objects)
#define CHECKSUM_LENGTH 1
#define MAX_PAYLOAD_LENGTH 256
#define MAX_PACKET_LENGTH (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH)
// Private types
typedef enum {STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxState;
// Private variables
static UAVTalkOutputStream outStream;
static xSemaphoreHandle lock;
static xSemaphoreHandle transLock;
static xSemaphoreHandle respSema;
static UAVObjHandle respObj;
static uint16_t respInstId;
static uint8_t rxBuffer[MAX_PACKET_LENGTH];
static uint8_t txBuffer[MAX_PACKET_LENGTH];
static UAVTalkStats stats;
// Private functions
static int32_t objectTransaction(UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout);
static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type);
static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type);
static int32_t sendNack(uint32_t objId);
static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length);
static void updateAck(UAVObjHandle obj, uint16_t instId);
static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout);
static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId);
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length);
static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId);
/**
* Initialize the UAVTalk library
* \param[in] connection UAVTalkConnection to be used
* \param[in] outputStream Function pointer that is called to send a data buffer
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream)
UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream, uint32_t maxPacketSize)
{
outStream = outputStream;
lock = xSemaphoreCreateRecursiveMutex();
transLock = xSemaphoreCreateRecursiveMutex();
vSemaphoreCreateBinary(respSema);
xSemaphoreTake(respSema, 0); // reset to zero
UAVTalkResetStats();
if (maxPacketSize<1) return 0;
// allocate object
UAVTalkConnectionData * connection = pvPortMalloc(sizeof(UAVTalkConnectionData));
if (!connection) return 0;
connection->canari = UAVTALK_CANARI;
connection->iproc.rxPacketLength = 0;
connection->iproc.state = UAVTALK_STATE_SYNC;
connection->outStream = outputStream;
connection->lock = xSemaphoreCreateRecursiveMutex();
connection->transLock = xSemaphoreCreateRecursiveMutex();
connection->txSize = maxPacketSize;
// allocate buffers
connection->rxBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH);
if (!connection->rxBuffer) return 0;
connection->txBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH);
if (!connection->txBuffer) return 0;
vSemaphoreCreateBinary(connection->respSema);
xSemaphoreTake(connection->respSema, 0); // reset to zero
UAVTalkResetStats( (UAVTalkConnection) connection );
return (UAVTalkConnection) connection;
}
/**
* Set the communication output stream
* \param[in] connection UAVTalkConnection to be used
* \param[in] outputStream Function pointer that is called to send a data buffer
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkSetOutputStream(UAVTalkConnection connectionHandle, UAVTalkOutputStream outputStream)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return -1);
// Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// set output stream
connection->outStream = outputStream;
// Release lock
xSemaphoreGiveRecursive(connection->lock);
return 0;
}
/**
* Get current output stream
* \param[in] connection UAVTalkConnection to be used
* @return UAVTarlkOutputStream the output stream used
*/
UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connectionHandle)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return NULL);
return connection->outStream;
}
/**
* Get communication statistics counters
* \param[in] connection UAVTalkConnection to be used
* @param[out] statsOut Statistics counters
*/
void UAVTalkGetStats(UAVTalkStats* statsOut)
void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats* statsOut)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return );
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// Copy stats
memcpy(statsOut, &stats, sizeof(UAVTalkStats));
memcpy(statsOut, &connection->stats, sizeof(UAVTalkStats));
// Release lock
xSemaphoreGiveRecursive(lock);
xSemaphoreGiveRecursive(connection->lock);
}
/**
* Reset the statistics counters.
* \param[in] connection UAVTalkConnection to be used
*/
void UAVTalkResetStats()
void UAVTalkResetStats(UAVTalkConnection connectionHandle)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return);
// Lock
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
// Clear stats
memset(&stats, 0, sizeof(UAVTalkStats));
memset(&connection->stats, 0, sizeof(UAVTalkStats));
// Release lock
xSemaphoreGiveRecursive(lock);
xSemaphoreGiveRecursive(connection->lock);
}
/**
* Request an update for the specified object, on success the object data would have been
* updated by the GCS.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object to update
* \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] timeout Time to wait for the response, when zero it will return immediately
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t timeout)
int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, int32_t timeout)
{
return objectTransaction(obj, instId, TYPE_OBJ_REQ, timeout);
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return -1);
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout);
}
/**
* Send the specified object through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object to send
* \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] acked Selects if an ack is required (1:ack required, 0: ack not required)
@ -144,69 +176,72 @@ int32_t UAVTalkSendObjectRequest(UAVObjHandle obj, uint16_t instId, int32_t time
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkSendObject(UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs)
int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs)
{
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return -1);
// Send object
if (acked == 1)
{
return objectTransaction(obj, instId, TYPE_OBJ_ACK, timeoutMs);
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs);
}
else
{
return objectTransaction(obj, instId, TYPE_OBJ, timeoutMs);
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs);
}
}
/**
* Execute the requested transaction on an object.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] type Transaction type
* TYPE_OBJ: send object,
* TYPE_OBJ_REQ: request object update
* TYPE_OBJ_ACK: send object with an ack
* UAVTALK_TYPE_OBJ: send object,
* UAVTALK_TYPE_OBJ_REQ: request object update
* UAVTALK_TYPE_OBJ_ACK: send object with an ack
* \return 0 Success
* \return -1 Failure
*/
static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs)
static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs)
{
int32_t respReceived;
// Send object depending on if a response is needed
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ)
if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_REQ)
{
// Get transaction lock (will block if a transaction is pending)
xSemaphoreTakeRecursive(transLock, portMAX_DELAY);
xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);
// Send object
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
respObj = obj;
respInstId = instId;
sendObject(obj, instId, type);
xSemaphoreGiveRecursive(lock);
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
connection->respObj = obj;
connection->respInstId = instId;
sendObject(connection, obj, instId, type);
xSemaphoreGiveRecursive(connection->lock);
// Wait for response (or timeout)
respReceived = xSemaphoreTake(respSema, timeoutMs/portTICK_RATE_MS);
respReceived = xSemaphoreTake(connection->respSema, timeoutMs/portTICK_RATE_MS);
// Check if a response was received
if (respReceived == pdFALSE)
{
// Cancel transaction
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
xSemaphoreTake(respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema)
respObj = 0;
xSemaphoreGiveRecursive(lock);
xSemaphoreGiveRecursive(transLock);
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema)
connection->respObj = 0;
xSemaphoreGiveRecursive(connection->lock);
xSemaphoreGiveRecursive(connection->transLock);
return -1;
}
else
{
xSemaphoreGiveRecursive(transLock);
xSemaphoreGiveRecursive(connection->transLock);
return 0;
}
}
else if (type == TYPE_OBJ)
else if (type == UAVTALK_TYPE_OBJ)
{
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
sendObject(obj, instId, TYPE_OBJ);
xSemaphoreGiveRecursive(lock);
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ);
xSemaphoreGiveRecursive(connection->lock);
return 0;
}
else
@ -217,222 +252,214 @@ static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type
/**
* Process an byte from the telemetry stream.
* \param[in] connection UAVTalkConnection to be used
* \param[in] rxbyte Received byte
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte)
{
static UAVObjHandle obj;
static uint8_t type;
static uint16_t packet_size;
static uint32_t objId;
static uint16_t instId;
static uint32_t length;
static uint8_t cs, csRx;
static int32_t rxCount;
static RxState state = STATE_SYNC;
static uint16_t rxPacketLength = 0;
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return -1);
UAVTalkInputProcessor *iproc = &connection->iproc;
++connection->stats.rxBytes;
++stats.rxBytes;
if (rxPacketLength < 0xffff)
rxPacketLength++; // update packet byte count
if (iproc->rxPacketLength < 0xffff)
iproc->rxPacketLength++; // update packet byte count
// Receive state machine
switch (state)
switch (iproc->state)
{
case STATE_SYNC:
if (rxbyte != SYNC_VAL)
case UAVTALK_STATE_SYNC:
if (rxbyte != UAVTALK_SYNC_VAL)
break;
// Initialize and update the CRC
cs = PIOS_CRC_updateByte(0, rxbyte);
iproc->cs = PIOS_CRC_updateByte(0, rxbyte);
rxPacketLength = 1;
iproc->rxPacketLength = 1;
state = STATE_TYPE;
iproc->state = UAVTALK_STATE_TYPE;
break;
case STATE_TYPE:
case UAVTALK_STATE_TYPE:
// update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte);
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if ((rxbyte & TYPE_MASK) != TYPE_VER)
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER)
{
state = STATE_SYNC;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
type = rxbyte;
iproc->type = rxbyte;
packet_size = 0;
iproc->packet_size = 0;
state = STATE_SIZE;
rxCount = 0;
iproc->state = UAVTALK_STATE_SIZE;
iproc->rxCount = 0;
break;
case STATE_SIZE:
case UAVTALK_STATE_SIZE:
// update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte);
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
if (rxCount == 0)
if (iproc->rxCount == 0)
{
packet_size += rxbyte;
rxCount++;
iproc->packet_size += rxbyte;
iproc->rxCount++;
break;
}
packet_size += rxbyte << 8;
iproc->packet_size += rxbyte << 8;
if (packet_size < MIN_HEADER_LENGTH || packet_size > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH)
if (iproc->packet_size < UAVTALK_MIN_HEADER_LENGTH || iproc->packet_size > UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH)
{ // incorrect packet size
state = STATE_SYNC;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
rxCount = 0;
objId = 0;
state = STATE_OBJID;
iproc->rxCount = 0;
iproc->objId = 0;
iproc->state = UAVTALK_STATE_OBJID;
break;
case STATE_OBJID:
case UAVTALK_STATE_OBJID:
// update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte);
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
objId += rxbyte << (8*(rxCount++));
iproc->objId += rxbyte << (8*(iproc->rxCount++));
if (rxCount < 4)
if (iproc->rxCount < 4)
break;
// Search for object, if not found reset state machine
// except if we got a OBJ_REQ for an object which does not
// exist, in which case we'll send a NACK
obj = UAVObjGetByID(objId);
if (obj == 0 && type != TYPE_OBJ_REQ)
iproc->obj = UAVObjGetByID(iproc->objId);
if (iproc->obj == 0 && iproc->type != UAVTALK_TYPE_OBJ_REQ)
{
stats.rxErrors++;
state = STATE_SYNC;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
// Determine data length
if (type == TYPE_OBJ_REQ || type == TYPE_ACK || type == TYPE_NACK)
length = 0;
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK)
iproc->length = 0;
else
length = UAVObjGetNumBytes(obj);
iproc->length = UAVObjGetNumBytes(iproc->obj);
// Check length and determine next state
if (length >= MAX_PAYLOAD_LENGTH)
if (iproc->length >= UAVTALK_MAX_PAYLOAD_LENGTH)
{
stats.rxErrors++;
state = STATE_SYNC;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
// Check the lengths match
if ((rxPacketLength + length) != packet_size)
if ((iproc->rxPacketLength + iproc->length) != iproc->packet_size)
{ // packet error - mismatched packet size
stats.rxErrors++;
state = STATE_SYNC;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
instId = 0;
if (obj == 0)
iproc->instId = 0;
if (iproc->obj == 0)
{
// If this is a NACK, we skip to Checksum
state = STATE_CS;
rxCount = 0;
iproc->state = UAVTALK_STATE_CS;
iproc->rxCount = 0;
}
// Check if this is a single instance object (i.e. if the instance ID field is coming next)
else if (UAVObjIsSingleInstance(obj))
else if (UAVObjIsSingleInstance(iproc->obj))
{
// If there is a payload get it, otherwise receive checksum
if (length > 0)
state = STATE_DATA;
if (iproc->length > 0)
iproc->state = UAVTALK_STATE_DATA;
else
state = STATE_CS;
iproc->state = UAVTALK_STATE_CS;
rxCount = 0;
iproc->rxCount = 0;
}
else
{
state = STATE_INSTID;
rxCount = 0;
iproc->state = UAVTALK_STATE_INSTID;
iproc->rxCount = 0;
}
break;
case STATE_INSTID:
case UAVTALK_STATE_INSTID:
// update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte);
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
instId += rxbyte << (8*(rxCount++));
iproc->instId += rxbyte << (8*(iproc->rxCount++));
if (rxCount < 2)
if (iproc->rxCount < 2)
break;
rxCount = 0;
iproc->rxCount = 0;
// If there is a payload get it, otherwise receive checksum
if (length > 0)
state = STATE_DATA;
if (iproc->length > 0)
iproc->state = UAVTALK_STATE_DATA;
else
state = STATE_CS;
iproc->state = UAVTALK_STATE_CS;
break;
case STATE_DATA:
case UAVTALK_STATE_DATA:
// update the CRC
cs = PIOS_CRC_updateByte(cs, rxbyte);
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
rxBuffer[rxCount++] = rxbyte;
if (rxCount < length)
connection->rxBuffer[iproc->rxCount++] = rxbyte;
if (iproc->rxCount < iproc->length)
break;
state = STATE_CS;
rxCount = 0;
iproc->state = UAVTALK_STATE_CS;
iproc->rxCount = 0;
break;
case STATE_CS:
case UAVTALK_STATE_CS:
// the CRC byte
csRx = rxbyte;
if (csRx != cs)
if (rxbyte != iproc->cs)
{ // packet error - faulty CRC
stats.rxErrors++;
state = STATE_SYNC;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
if (rxPacketLength != (packet_size + 1))
if (iproc->rxPacketLength != (iproc->packet_size + 1))
{ // packet error - mismatched packet size
stats.rxErrors++;
state = STATE_SYNC;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
break;
}
xSemaphoreTakeRecursive(lock, portMAX_DELAY);
receiveObject(type, objId, instId, rxBuffer, length);
stats.rxObjectBytes += length;
stats.rxObjects++;
xSemaphoreGiveRecursive(lock);
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length);
connection->stats.rxObjectBytes += iproc->length;
connection->stats.rxObjects++;
xSemaphoreGiveRecursive(connection->lock);
state = STATE_SYNC;
iproc->state = UAVTALK_STATE_SYNC;
break;
default:
stats.rxErrors++;
state = STATE_SYNC;
connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC;
}
// Done
@ -441,7 +468,8 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
/**
* Receive an object. This function process objects received through the telemetry stream.
* \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK)
* \param[in] connection UAVTalkConnection to be used
* \param[in] type Type of received message (UAVTALK_TYPE_OBJ, UAVTALK_TYPE_OBJ_REQ, UAVTALK_TYPE_OBJ_ACK, UAVTALK_TYPE_ACK, UAVTALK_TYPE_NACK)
* \param[in] objId ID of the object to work on
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] data Data buffer
@ -449,9 +477,9 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
* \return 0 Success
* \return -1 Failure
*/
static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length)
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length)
{
static UAVObjHandle obj;
UAVObjHandle obj;
int32_t ret = 0;
// Get the handle to the Object. Will be zero
@ -460,21 +488,21 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint
// Process message type
switch (type) {
case TYPE_OBJ:
case UAVTALK_TYPE_OBJ:
// All instances, not allowed for OBJ messages
if (instId != UAVOBJ_ALL_INSTANCES)
{
// Unpack object, if the instance does not exist it will be created!
UAVObjUnpack(obj, instId, data);
// Check if an ack is pending
updateAck(obj, instId);
updateAck(connection, obj, instId);
}
else
{
ret = -1;
}
break;
case TYPE_OBJ_ACK:
case UAVTALK_TYPE_OBJ_ACK:
// All instances, not allowed for OBJ_ACK messages
if (instId != UAVOBJ_ALL_INSTANCES)
{
@ -482,7 +510,7 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint
if ( UAVObjUnpack(obj, instId, data) == 0 )
{
// Transmit ACK
sendObject(obj, instId, TYPE_ACK);
sendObject(connection, obj, instId, UAVTALK_TYPE_ACK);
}
else
{
@ -494,22 +522,22 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint
ret = -1;
}
break;
case TYPE_OBJ_REQ:
case UAVTALK_TYPE_OBJ_REQ:
// Send requested object if message is of type OBJ_REQ
if (obj == 0)
sendNack(objId);
sendNack(connection, objId);
else
sendObject(obj, instId, TYPE_OBJ);
sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ);
break;
case TYPE_NACK:
case UAVTALK_TYPE_NACK:
// Do nothing on flight side, let it time out.
break;
case TYPE_ACK:
case UAVTALK_TYPE_ACK:
// All instances, not allowed for ACK messages
if (instId != UAVOBJ_ALL_INSTANCES)
{
// Check if an ack is pending
updateAck(obj, instId);
updateAck(connection, obj, instId);
}
else
{
@ -525,25 +553,29 @@ static int32_t receiveObject(uint8_t type, uint32_t objId, uint16_t instId, uint
/**
* Check if an ack is pending on an object and give response semaphore
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
*/
static void updateAck(UAVObjHandle obj, uint16_t instId)
static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId)
{
if (respObj == obj && (respInstId == instId || respInstId == UAVOBJ_ALL_INSTANCES))
if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES))
{
xSemaphoreGive(respSema);
respObj = 0;
xSemaphoreGive(connection->respSema);
connection->respObj = 0;
}
}
/**
* Send an object through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object handle to send
* \param[in] instId The instance ID or UAVOBJ_ALL_INSTANCES for all instances
* \param[in] type Transaction type
* \return 0 Success
* \return -1 Failure
*/
static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
{
uint32_t numInst;
uint32_t n;
@ -555,7 +587,7 @@ static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
}
// Process message type
if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK )
if ( type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_ACK )
{
if (instId == UAVOBJ_ALL_INSTANCES)
{
@ -564,24 +596,24 @@ static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
// Send all instances
for (n = 0; n < numInst; ++n)
{
sendSingleObject(obj, n, type);
sendSingleObject(connection, obj, n, type);
}
return 0;
}
else
{
return sendSingleObject(obj, instId, type);
return sendSingleObject(connection, obj, instId, type);
}
}
else if (type == TYPE_OBJ_REQ)
else if (type == UAVTALK_TYPE_OBJ_REQ)
{
return sendSingleObject(obj, instId, TYPE_OBJ_REQ);
return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ);
}
else if (type == TYPE_ACK)
else if (type == UAVTALK_TYPE_ACK)
{
if ( instId != UAVOBJ_ALL_INSTANCES )
{
return sendSingleObject(obj, instId, TYPE_ACK);
return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK);
}
else
{
@ -596,13 +628,14 @@ static int32_t sendObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
/**
* Send an object through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] obj Object handle to send
* \param[in] instId The instance ID (can NOT be UAVOBJ_ALL_INSTANCES, use sendObject() instead)
* \param[in] type Transaction type
* \return 0 Success
* \return -1 Failure
*/
static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
{
int32_t length;
int32_t dataOffset;
@ -610,13 +643,13 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
// Setup type and object id fields
objId = UAVObjGetID(obj);
txBuffer[0] = SYNC_VAL; // sync byte
txBuffer[1] = type;
connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
connection->txBuffer[1] = type;
// data length inserted here below
txBuffer[4] = (uint8_t)(objId & 0xFF);
txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
// Setup instance ID if one is required
if (UAVObjIsSingleInstance(obj))
@ -625,13 +658,13 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
}
else
{
txBuffer[8] = (uint8_t)(instId & 0xFF);
txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
connection->txBuffer[8] = (uint8_t)(instId & 0xFF);
connection->txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
dataOffset = 10;
}
// Determine data length
if (type == TYPE_OBJ_REQ || type == TYPE_ACK)
if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK)
{
length = 0;
}
@ -641,7 +674,7 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
}
// Check length
if (length >= MAX_PAYLOAD_LENGTH)
if (length >= UAVTALK_MAX_PAYLOAD_LENGTH)
{
return -1;
}
@ -649,26 +682,34 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
// Copy data (if any)
if (length > 0)
{
if ( UAVObjPack(obj, instId, &txBuffer[dataOffset]) < 0 )
if ( UAVObjPack(obj, instId, &connection->txBuffer[dataOffset]) < 0 )
{
return -1;
}
}
// Store the packet length
txBuffer[2] = (uint8_t)((dataOffset+length) & 0xFF);
txBuffer[3] = (uint8_t)(((dataOffset+length) >> 8) & 0xFF);
connection->txBuffer[2] = (uint8_t)((dataOffset+length) & 0xFF);
connection->txBuffer[3] = (uint8_t)(((dataOffset+length) >> 8) & 0xFF);
// Calculate checksum
txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, txBuffer, dataOffset+length);
// Send buffer
if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+length+CHECKSUM_LENGTH);
connection->txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset+length);
// Send buffer (partially if needed)
uint32_t sent=0;
while (sent < dataOffset+length+UAVTALK_CHECKSUM_LENGTH) {
uint32_t sending = dataOffset+length+UAVTALK_CHECKSUM_LENGTH - sent;
if ( sending > connection->txSize ) sending = connection->txSize;
if ( connection->outStream != NULL ) {
(*connection->outStream)(connection->txBuffer+sent, sending);
}
sent += sending;
}
// Update stats
++stats.txObjects;
stats.txBytes += dataOffset+length+CHECKSUM_LENGTH;
stats.txObjectBytes += length;
++connection->stats.txObjects;
connection->stats.txBytes += dataOffset+length+UAVTALK_CHECKSUM_LENGTH;
connection->stats.txObjectBytes += length;
// Done
return 0;
@ -676,36 +717,37 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
/**
* Send a NACK through the telemetry link.
* \param[in] connection UAVTalkConnection to be used
* \param[in] objId Object ID to send a NACK for
* \return 0 Success
* \return -1 Failure
*/
static int32_t sendNack(uint32_t objId)
static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId)
{
int32_t dataOffset;
txBuffer[0] = SYNC_VAL; // sync byte
txBuffer[1] = TYPE_NACK;
connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
connection->txBuffer[1] = UAVTALK_TYPE_NACK;
// data length inserted here below
txBuffer[4] = (uint8_t)(objId & 0xFF);
txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
connection->txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
connection->txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
dataOffset = 8;
// Store the packet length
txBuffer[2] = (uint8_t)((dataOffset) & 0xFF);
txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF);
connection->txBuffer[2] = (uint8_t)((dataOffset) & 0xFF);
connection->txBuffer[3] = (uint8_t)(((dataOffset) >> 8) & 0xFF);
// Calculate checksum
txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, txBuffer, dataOffset);
connection->txBuffer[dataOffset] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset);
// Send buffer
if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+CHECKSUM_LENGTH);
if (connection->outStream!=NULL) (*connection->outStream)(connection->txBuffer, dataOffset+UAVTALK_CHECKSUM_LENGTH);
// Update stats
stats.txBytes += dataOffset+CHECKSUM_LENGTH;
connection->stats.txBytes += dataOffset+UAVTALK_CHECKSUM_LENGTH;
// Done
return 0;

441
ground/openpilotgcs/src/plugins/config/camerastabilization.ui Normal file
View File

@ -0,0 +1,441 @@
<?xml version="1.0" encoding="UTF-8"?>
<ui version="4.0">
<class>CameraStabilizationWidget</class>
<widget class="QWidget" name="CameraStabilizationWidget">
<property name="geometry">
<rect>
<x>0</x>
<y>0</y>
<width>720</width>
<height>567</height>
</rect>
</property>
<property name="windowTitle">
<string>Form</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_2">
<item>
<widget class="QCheckBox" name="enableCameraStabilization">
<property name="text">
<string>Enable CameraStabilization module</string>
</property>
</widget>
</item>
<item>
<widget class="QLabel" name="label_7">
<property name="text">
<string>After enabling the module, you must power cycle before using and configuring.</string>
</property>
</widget>
</item>
<item>
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox">
<property name="title">
<string>Channel Ranges (number of degrees full range)</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<layout class="QFormLayout" name="formLayout_2">
<item row="2" column="0">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Pitch</string>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_3">
<property name="text">
<string>Yaw</string>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label">
<property name="text">
<string>Roll</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QSpinBox" name="rollOutputRange">
<property name="maximum">
<number>180</number>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QSpinBox" name="pitchOutputRange">
<property name="maximum">
<number>180</number>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QSpinBox" name="yawOutputRange">
<property name="maximum">
<number>180</number>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="horizontalSpacer_4">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<spacer name="horizontalSpacer_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>212</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="groupBox_2">
<property name="title">
<string>Channel Mapping (select output channel or none to disable)</string>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_2">
<item>
<layout class="QFormLayout" name="formLayout">
<item row="2" column="0">
<widget class="QLabel" name="label_4">
<property name="text">
<string>Roll</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QComboBox" name="rollChannel">
<item>
<property name="text">
<string>None</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 1</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 2</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 3</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 4</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 5</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 6</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 7</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 8</string>
</property>
</item>
</widget>
</item>
<item row="4" column="1">
<widget class="QComboBox" name="yawChannel">
<item>
<property name="text">
<string>None</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 1</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 2</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 3</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 4</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 5</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 6</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 7</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 8</string>
</property>
</item>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Pitch</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QComboBox" name="pitchChannel">
<item>
<property name="text">
<string>None</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 1</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 2</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 3</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 4</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 5</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 6</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 7</string>
</property>
</item>
<item>
<property name="text">
<string>Channel 8</string>
</property>
</item>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_6">
<property name="text">
<string>Yaw</string>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="horizontalSpacer_5">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>212</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
<height>40</height>
</size>
</property>
</spacer>
</item>
<item>
<layout class="QHBoxLayout" name="submitButtons">
<item>
<widget class="QFrame" name="frame">
<property name="frameShape">
<enum>QFrame::StyledPanel</enum>
</property>
<property name="frameShadow">
<enum>QFrame::Raised</enum>
</property>
</widget>
</item>
<item>
<spacer name="horizontalSpacer">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>40</width>
<height>20</height>
</size>
</property>
</spacer>
</item>
<item>
<widget class="QPushButton" name="camerastabilizationHelp">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
<width>32</width>
<height>32</height>
</size>
</property>
<property name="font">
<font>
<kerning>true</kerning>
</font>
</property>
<property name="text">
<string/>
</property>
<property name="icon">
<iconset resource="../coreplugin/core.qrc">
<normaloff>:/core/images/helpicon.svg</normaloff>:/core/images/helpicon.svg</iconset>
</property>
<property name="iconSize">
<size>
<width>32</width>
<height>32</height>
</size>
</property>
<property name="shortcut">
<string>Ctrl+S</string>
</property>
<property name="default">
<bool>false</bool>
</property>
<property name="flat">
<bool>true</bool>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="camerastabilizationSaveRAM">
<property name="toolTip">
<string>Save settings to the board (RAM only).
This does not save the calibration settings, this is done using the
specific calibration button on top of the screen.</string>
</property>
<property name="text">
<string>Apply</string>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="camerastabilizationSaveSD">
<property name="toolTip">
<string>Send settings to the board, and save to the non-volatile memory.</string>
</property>
<property name="text">
<string>Save</string>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
<resources>
<include location="../coreplugin/core.qrc"/>
</resources>
<connections/>
</ui>

13
ground/openpilotgcs/src/plugins/config/config.pro
View File

@ -37,7 +37,8 @@ HEADERS += configplugin.h \
calibration.h \
defaultattitudewidget.h \
smartsavebutton.h \
defaulthwsettingswidget.h
defaulthwsettingswidget.h \
configcamerastabilizationwidget.h
SOURCES += configplugin.cpp \
configgadgetconfiguration.cpp \
@ -65,7 +66,8 @@ SOURCES += configplugin.cpp \
alignment-calibration.cpp \
defaultattitudewidget.cpp \
smartsavebutton.cpp \
defaulthwsettingswidget.cpp
defaulthwsettingswidget.cpp \
configcamerastabilizationwidget.cpp
FORMS += \
airframe.ui \
@ -78,6 +80,11 @@ FORMS += \
output.ui \
ccattitude.ui \
defaultattitude.ui \
defaulthwsettings.ui
defaulthwsettings.ui \
camerastabilization.ui
RESOURCES += configgadget.qrc

237
ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.cpp Normal file
View File

@ -0,0 +1,237 @@
/**
******************************************************************************
*
* @file configcamerastabilizationwidget.cpp
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief The Configuration Gadget used to update settings in the firmware
*****************************************************************************/
/*
* 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 "configcamerastabilizationwidget.h"
#include <QDebug>
#include <QTimer>
#include <QStringList>
#include <QtGui/QWidget>
#include <QtGui/QTextEdit>
#include <QtGui/QVBoxLayout>
#include <QtGui/QPushButton>
#include <QThread>
#include <iostream>
#include <QUrl>
#include <QDesktopServices>
#include "camerastabsettings.h"
#include "hwsettings.h"
#include "mixersettings.h"
ConfigCameraStabilizationWidget::ConfigCameraStabilizationWidget(QWidget *parent) : ConfigTaskWidget(parent)
{
m_camerastabilization = new Ui_CameraStabilizationWidget();
m_camerastabilization->setupUi(this);
connectUpdates();
// Connect buttons
connect(m_camerastabilization->camerastabilizationSaveRAM,SIGNAL(clicked()),this,SLOT(applySettings()));
connect(m_camerastabilization->camerastabilizationSaveSD,SIGNAL(clicked()),this,SLOT(saveSettings()));
connect(m_camerastabilization->camerastabilizationHelp, SIGNAL(clicked()), this, SLOT(openHelp()));
}
ConfigCameraStabilizationWidget::~ConfigCameraStabilizationWidget()
{
// Do nothing
}
void ConfigCameraStabilizationWidget::connectUpdates()
{
// Now connect the widget to the StabilizationSettings object
connect(MixerSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
connect(CameraStabSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
// TODO: This will need to support both CC and OP later
connect(HwSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
}
void ConfigCameraStabilizationWidget::disconnectUpdates()
{
// Now connect the widget to the StabilizationSettings object
disconnect(MixerSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
disconnect(CameraStabSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
// TODO: This will need to support both CC and OP later
disconnect(HwSettings::GetInstance(getObjectManager()),SIGNAL(objectUpdated(UAVObject*)),this,SLOT(refreshValues()));
}
/**
* @brief Populate the gui settings into the appropriate
* UAV structures
*/
void ConfigCameraStabilizationWidget::applySettings()
{
// Enable or disable the settings
HwSettings * hwSettings = HwSettings::GetInstance(getObjectManager());
HwSettings::DataFields hwSettingsData = hwSettings->getData();
hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTABILIZATION] =
m_camerastabilization->enableCameraStabilization->isChecked() ?
HwSettings::OPTIONALMODULES_ENABLED :
HwSettings::OPTIONALMODULES_DISABLED;
// Update the mixer settings
MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager());
MixerSettings::DataFields mixerSettingsData = mixerSettings->getData();
const int NUM_MIXERS = 8;
QComboBox * selectors[3] = {
m_camerastabilization->rollChannel,
m_camerastabilization->pitchChannel,
m_camerastabilization->yawChannel
};
// TODO: Need to reformat object so types are an
// array themselves. This gets really awkward
quint8 * mixerTypes[NUM_MIXERS] = {
&mixerSettingsData.Mixer1Type,
&mixerSettingsData.Mixer2Type,
&mixerSettingsData.Mixer3Type,
&mixerSettingsData.Mixer4Type,
&mixerSettingsData.Mixer5Type,
&mixerSettingsData.Mixer6Type,
&mixerSettingsData.Mixer7Type,
&mixerSettingsData.Mixer8Type,
};
for (int i = 0; i < 3; i++)
{
// Channel 1 is second entry, so becomes zero
int mixerNum = selectors[i]->currentIndex() - 1;
if ( mixerNum >= 0 && // Short circuit in case of none
*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_DISABLED &&
(*mixerTypes[mixerNum] != MixerSettings::MIXER1TYPE_CAMERAROLL + i) ) {
// If the mixer channel already to something that isn't what we are
// about to set it to reset to none
selectors[i]->setCurrentIndex(0);
} else {
// Make sure no other channels have this output set
for (int j = 0; j < NUM_MIXERS; j++)
if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLL + i))
*mixerTypes[j] = MixerSettings::MIXER1TYPE_DISABLED;
// If this channel is assigned to one of the outputs that is not disabled
// set it
if(mixerNum >= 0 && mixerNum < NUM_MIXERS)
*mixerTypes[mixerNum] = MixerSettings::MIXER1TYPE_CAMERAROLL + i;
}
}
// Update the ranges
CameraStabSettings * cameraStab = CameraStabSettings::GetInstance(getObjectManager());
CameraStabSettings::DataFields cameraStabData = cameraStab->getData();
cameraStabData.OutputRange[CameraStabSettings::OUTPUTRANGE_ROLL] = m_camerastabilization->rollOutputRange->value();
cameraStabData.OutputRange[CameraStabSettings::OUTPUTRANGE_PITCH] = m_camerastabilization->pitchOutputRange->value();
cameraStabData.OutputRange[CameraStabSettings::OUTPUTRANGE_YAW] = m_camerastabilization->yawOutputRange->value();
// Because multiple objects are updated, and all of them trigger the callback
// they must be done together (if update one then load settings from second
// the first update would wipe the UI controls). However to be extra cautious
// I'm also disabling updates during the setting to the UAVObjects
disconnectUpdates();
hwSettings->setData(hwSettingsData);
mixerSettings->setData(mixerSettingsData);
cameraStab->setData(cameraStabData);
connectUpdates();
}
/**
* Push settings into UAV objects then save them
*/
void ConfigCameraStabilizationWidget::saveSettings()
{
applySettings();
UAVObject * obj = HwSettings::GetInstance(getObjectManager());
saveObjectToSD(obj);
obj = MixerSettings::GetInstance(getObjectManager());
saveObjectToSD(obj);
obj = CameraStabSettings::GetInstance(getObjectManager());
saveObjectToSD(obj);
}
void ConfigCameraStabilizationWidget::refreshValues()
{
HwSettings * hwSettings = HwSettings::GetInstance(getObjectManager());
HwSettings::DataFields hwSettingsData = hwSettings->getData();
m_camerastabilization->enableCameraStabilization->setChecked(
hwSettingsData.OptionalModules[HwSettings::OPTIONALMODULES_CAMERASTABILIZATION] ==
HwSettings::OPTIONALMODULES_ENABLED);
CameraStabSettings * cameraStabSettings = CameraStabSettings::GetInstance(getObjectManager());
CameraStabSettings::DataFields cameraStab = cameraStabSettings->getData();
m_camerastabilization->rollOutputRange->setValue(cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_ROLL]);
m_camerastabilization->pitchOutputRange->setValue(cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_PITCH]);
m_camerastabilization->yawOutputRange->setValue(cameraStab.OutputRange[CameraStabSettings::OUTPUTRANGE_YAW]);
MixerSettings * mixerSettings = MixerSettings::GetInstance(getObjectManager());
MixerSettings::DataFields mixerSettingsData = mixerSettings->getData();
const int NUM_MIXERS = 8;
QComboBox * selectors[3] = {
m_camerastabilization->rollChannel,
m_camerastabilization->pitchChannel,
m_camerastabilization->yawChannel
};
// TODO: Need to reformat object so types are an
// array themselves. This gets really awkward
quint8 * mixerTypes[NUM_MIXERS] = {
&mixerSettingsData.Mixer1Type,
&mixerSettingsData.Mixer2Type,
&mixerSettingsData.Mixer3Type,
&mixerSettingsData.Mixer4Type,
&mixerSettingsData.Mixer5Type,
&mixerSettingsData.Mixer6Type,
&mixerSettingsData.Mixer7Type,
&mixerSettingsData.Mixer8Type,
};
for (int i = 0; i < 3; i++)
{
// Default to none if not found. Then search for any mixer channels set to
// this
selectors[i]->setCurrentIndex(0);
for (int j = 0; j < NUM_MIXERS; j++)
if (*mixerTypes[j] == (MixerSettings::MIXER1TYPE_CAMERAROLL + i) &&
selectors[i]->currentIndex() != (j + 1))
selectors[i]->setCurrentIndex(j + 1);
}
}
void ConfigCameraStabilizationWidget::openHelp()
{
QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/Camera+Stabilization", QUrl::StrictMode) );
}
void ConfigCameraStabilizationWidget::enableControls(bool enable)
{
m_camerastabilization->camerastabilizationSaveSD->setEnabled(enable);
m_camerastabilization->camerastabilizationSaveRAM->setEnabled(enable);
}
/**
@}
@}
*/

68
ground/openpilotgcs/src/plugins/config/configcamerastabilizationwidget.h Normal file
View File

@ -0,0 +1,68 @@
/**
******************************************************************************
*
* @file configahrstwidget.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @addtogroup GCSPlugins GCS Plugins
* @{
* @addtogroup ConfigPlugin Config Plugin
* @{
* @brief Telemetry configuration panel
*****************************************************************************/
/*
* 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 CONFIGCAMERASTABILIZATIONWIDGET_H
#define CONFIGCAMERASTABILIZATIONWIDGET_H
#include "ui_camerastabilization.h"
#include "configtaskwidget.h"
#include "extensionsystem/pluginmanager.h"
#include "uavobjectmanager.h"
#include "uavobject.h"
#include <QtGui/QWidget>
#include <QtSvg/QSvgRenderer>
#include <QtSvg/QGraphicsSvgItem>
#include <QList>
#include <QTimer>
#include <QMutex>
#include "camerastabsettings.h"
class ConfigCameraStabilizationWidget: public ConfigTaskWidget
{
Q_OBJECT
public:
ConfigCameraStabilizationWidget(QWidget *parent = 0);
~ConfigCameraStabilizationWidget();
private:
virtual void enableControls(bool enable);
Ui_CameraStabilizationWidget *m_camerastabilization;
private slots:
void openHelp();
void applySettings();
void saveSettings();
void refreshValues();
protected:
void connectUpdates();
void disconnectUpdates();
};
#endif // ConfigCameraStabilization_H

1
ground/openpilotgcs/src/plugins/config/configgadget.qrc
View File

@ -15,5 +15,6 @@
<file>images/coptercontrol.svg</file>
<file>images/hw_config.png</file>
<file>images/gyroscope.png</file>
<file>images/camera.png</file>
</qresource>
</RCC>

3
ground/openpilotgcs/src/plugins/config/configgadgetwidget.cpp
View File

@ -33,6 +33,7 @@
#include "configinputwidget.h"
#include "configoutputwidget.h"
#include "configstabilizationwidget.h"
#include "configcamerastabilizationwidget.h"
#include "config_pro_hw_widget.h"
#include "config_cc_hw_widget.h"
#include "defaultattitudewidget.h"
@ -81,6 +82,8 @@ ConfigGadgetWidget::ConfigGadgetWidget(QWidget *parent) : QWidget(parent)
qwd = new ConfigStabilizationWidget(this);
ftw->insertTab(ConfigGadgetWidget::stabilization, qwd, QIcon(":/configgadget/images/gyroscope.png"), QString("Stabilization"));
qwd = new ConfigCameraStabilizationWidget(this);
ftw->insertTab(ConfigGadgetWidget::camerastabilization, qwd, QIcon(":/configgadget/images/camera.png"), QString("Camera Stab"));
// qwd = new ConfigPipXtremeWidget(this);

2
ground/openpilotgcs/src/plugins/config/configgadgetwidget.h
View File

@ -50,7 +50,7 @@ class ConfigGadgetWidget: public QWidget
public:
ConfigGadgetWidget(QWidget *parent = 0);
~ConfigGadgetWidget();
enum widgetTabs {hardware=0, aircraft, input, output, ins, stabilization};
enum widgetTabs {hardware=0, aircraft, input, output, ins, stabilization, camerastabilization};
public slots:
void onAutopilotConnect();

BIN
ground/openpilotgcs/src/plugins/config/images/camera.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 2.8 KiB

2
ground/openpilotgcs/src/plugins/coreplugin/mainwindow.cpp
View File

@ -179,6 +179,8 @@ MainWindow::MainWindow() :
m_modeStack->setIconSize(QSize(24,24));
m_modeStack->setTabPosition(QTabWidget::South);
m_modeStack->setMovable(false);
m_modeStack->setMinimumWidth(512);
m_modeStack->setElideMode(Qt::ElideRight);
#ifndef Q_WS_MAC
m_modeStack->setDocumentMode(true);
#endif

16
ground/openpilotgcs/src/plugins/modelview/modelviewgadgetwidget.cpp
View File

@ -291,16 +291,12 @@ void ModelViewGadgetWidget::updateAttitude()
{
AttitudeActual::DataFields data = attActual->getData(); // get attitude data
GLC_StructOccurence* rootObject= m_World.rootOccurence(); // get the full 3D model
// create quaternions rotations for each axis
QQuaternion qX= QQuaternion::fromAxisAndAngle(QVector3D(1,0,0),data.Pitch);
QQuaternion qY= QQuaternion::fromAxisAndAngle(QVector3D(0,1,0),data.Roll);
QQuaternion qZ= QQuaternion::fromAxisAndAngle(QVector3D(0,0,1),data.Yaw);
QQuaternion quat= qX * qY * qZ; // create the quaternion of all the rotations
// pass values to simpler variables
double x= quat.vector().x();
double y= quat.vector().y();
double z= quat.vector().z();
double w= quat.scalar();
double x= data.q3;
double y= data.q2;
double z= data.q4;
double w= data.q1;
if (w == 0.0)
w = 1.0;
// create and gives the product of 2 4x4 matrices to get the rotation of the 3D model's matrix
QMatrix4x4 m1;
m1.setRow(0, QVector4D(w,z,-y,x));

8
ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro
View File

@ -31,6 +31,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/ahrssettings.h \
$$UAVOBJECT_SYNTHETICS/gcstelemetrystats.h \
$$UAVOBJECT_SYNTHETICS/attituderaw.h \
$$UAVOBJECT_SYNTHETICS/camerastabsettings.h \
$$UAVOBJECT_SYNTHETICS/flighttelemetrystats.h \
$$UAVOBJECT_SYNTHETICS/systemstats.h \
$$UAVOBJECT_SYNTHETICS/systemalarms.h \
@ -69,7 +70,8 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
$$UAVOBJECT_SYNTHETICS/sonaraltitude.h \
$$UAVOBJECT_SYNTHETICS/flightstatus.h \
$$UAVOBJECT_SYNTHETICS/hwsettings.h \
$$UAVOBJECT_SYNTHETICS/attitudesettings.h
$$UAVOBJECT_SYNTHETICS/attitudesettings.h \
$$UAVOBJECT_SYNTHETICS/cameradesired.h
SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/ahrsstatus.cpp \
@ -79,6 +81,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/ahrssettings.cpp \
$$UAVOBJECT_SYNTHETICS/gcstelemetrystats.cpp \
$$UAVOBJECT_SYNTHETICS/attituderaw.cpp \
$$UAVOBJECT_SYNTHETICS/camerastabsettings.cpp \
$$UAVOBJECT_SYNTHETICS/flighttelemetrystats.cpp \
$$UAVOBJECT_SYNTHETICS/systemstats.cpp \
$$UAVOBJECT_SYNTHETICS/systemalarms.cpp \
@ -118,4 +121,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
$$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \
$$UAVOBJECT_SYNTHETICS/flightstatus.cpp \
$$UAVOBJECT_SYNTHETICS/hwsettings.cpp \
$$UAVOBJECT_SYNTHETICS/attitudesettings.cpp
$$UAVOBJECT_SYNTHETICS/attitudesettings.cpp \
$$UAVOBJECT_SYNTHETICS/cameradesired.cpp

26
ground/openpilotgcs/src/plugins/uploader/devicewidget.cpp
View File

@ -189,7 +189,7 @@ bool deviceWidget::populateBoardStructuredDescription(QByteArray desc)
myDevice->lblDescription->setText(QString("Firmware tag: ")+onBoardDescrition.description);
QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Official Firmware Build"));
myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build"));
}
else
@ -197,7 +197,7 @@ bool deviceWidget::populateBoardStructuredDescription(QByteArray desc)
myDevice->lblDescription->setText(onBoardDescrition.description);
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Custom Firmware Build"));
myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build"));
}
myDevice->lblBrdName->setText(idToBoardName(onBoardDescrition.boardType<<8));
@ -220,7 +220,7 @@ bool deviceWidget::populateLoadedStructuredDescription(QByteArray desc)
myDevice->description->setText(LoadedDescrition.description);
QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg"));
myDevice->lblCertifiedL->setPixmap(pix);
myDevice->lblCertifiedL->setToolTip(tr("Official Firmware Build"));
myDevice->lblCertifiedL->setToolTip(tr("Tagged officially released firmware build"));
}
else
{
@ -228,7 +228,7 @@ bool deviceWidget::populateLoadedStructuredDescription(QByteArray desc)
myDevice->description->setText(LoadedDescrition.description);
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
myDevice->lblCertifiedL->setPixmap(pix);
myDevice->lblCertifiedL->setToolTip(tr("Custom Firmware Build"));
myDevice->lblCertifiedL->setToolTip(tr("Untagged or custom firmware build"));
}
myDevice->lblBrdNameL->setText(deviceDescriptorStruct::idToBoardName(LoadedDescrition.boardType<<8));
@ -312,34 +312,34 @@ void deviceWidget::loadFirmware()
myDevice->groupCustom->setVisible(false);
if(myDevice->lblCRC->text()==myDevice->lblCRCL->text())
{
myDevice->statusLabel->setText(tr("The loaded firmware the same as on the board. You should not upload it"));
myDevice->statusLabel->setText(tr("The board has the same firmware as loaded. No need to update"));
px.load(QString(":/uploader/images/warning.svg"));
}
else if(myDevice->lblDevName->text()!=myDevice->lblBrdNameL->text())
{
myDevice->statusLabel->setText(tr("The loaded firmware is not suited for the HW connected. You shouldn't upload it"));
px.load(QString(":/uploader/images/warning.svg"));
myDevice->statusLabel->setText(tr("WARNING: the loaded firmware is for different hardware. Do not update!"));
px.load(QString(":/uploader/images/error.svg"));
}
else if(QDateTime::fromString(onBoardDescrition.buildDate)>QDateTime::fromString(LoadedDescrition.buildDate))
{
myDevice->statusLabel->setText(tr("The loaded firmware is older than the firmware on the board. You should not upload it"));
myDevice->statusLabel->setText(tr("The board has newer firmware than loaded. Are you sure you want to update?"));
px.load(QString(":/uploader/images/warning.svg"));
}
else if(!LoadedDescrition.description.startsWith("release",Qt::CaseInsensitive))
{
myDevice->statusLabel->setText(tr("The loaded firmware is not an oficial OpenPilot release. You should upload it only if you know what you are doing"));
myDevice->statusLabel->setText(tr("The loaded firmware is untagged or custom build. Update only if it was received from a trusted source (official website or your own build)"));
px.load(QString(":/uploader/images/warning.svg"));
}
else
{
myDevice->statusLabel->setText(tr("Everything seems OK. You should upload the loaded firmware by pressing 'Flash'"));
myDevice->statusLabel->setText(tr("This is the tagged officially released OpenPilot firmware"));
px.load(QString(":/uploader/images/gtk-info.svg"));
}
}
else
{
myDevice->statusLabel->setText(tr("The loaded firmware was not packaged with the OpenPilot format. You should not upload it unless you know what you are doing."));
px.load(QString(":/uploader/images/warning.svg"));
myDevice->statusLabel->setText(tr("WARNING: the loaded firmware was not packaged with the OpenPilot format. Do not update unless you know what you are doing"));
px.load(QString(":/uploader/images/error.svg"));
myDevice->youdont->setChecked(false);
myDevice->youdont->setVisible(true);
myDevice->verticalGroupBox_loaded->setVisible(false);
@ -514,7 +514,7 @@ QString deviceWidget::setOpenFileName()
QString fileName = QFileDialog::getOpenFileName(this,
tr("Select firmware file"),
"",
tr("Firmware Files (*.bin *.opfw)"),
tr("Firmware Files (*.opfw *.bin)"),
&selectedFilter,
options);
return fileName;

10
ground/openpilotgcs/src/plugins/uploader/devicewidget.ui
View File

@ -93,10 +93,10 @@
<item>
<widget class="QPushButton" name="pbLoad">
<property name="toolTip">
<string>Loads the firmware</string>
<string>Open a file with new firmware image to be flashed</string>
</property>
<property name="text">
<string>Load...</string>
<string>Open...</string>
</property>
</widget>
</item>
@ -116,7 +116,7 @@
<bool>false</bool>
</property>
<property name="toolTip">
<string>Update the firmware on this board.</string>
<string>Write loaded firmware image to the board</string>
</property>
<property name="text">
<string>Flash</string>
@ -126,7 +126,7 @@
<item>
<widget class="QPushButton" name="retrieveButton">
<property name="toolTip">
<string>Download the current board firmware to your computer</string>
<string>Read and save current board firmware to a file</string>
</property>
<property name="text">
<string>Retrieve...</string>
@ -228,7 +228,7 @@
<item>
<widget class="QLabel" name="label_3">
<property name="text">
<string>GIT Tag:</string>
<string>Git tag:</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>

35
ground/openpilotgcs/src/plugins/uploader/images/error.svg Normal file
View File

@ -0,0 +1,35 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg id="svg3247" xmlns="http://www.w3.org/2000/svg" height="48" width="48" version="1.0" xmlns:xlink="http://www.w3.org/1999/xlink">
<defs id="defs3249">
<linearGradient id="linearGradient2411" y2="5.4676" gradientUnits="userSpaceOnUse" x2="63.397" gradientTransform="matrix(2.1154 0 0 2.1153 -107.58 32.427)" y1="-12.489" x1="63.397">
<stop id="stop4875" style="stop-color:#fff" offset="0"/>
<stop id="stop4877" style="stop-color:#fff;stop-opacity:0" offset="1"/>
</linearGradient>
<linearGradient id="linearGradient2416" y2="3.0816" gradientUnits="userSpaceOnUse" x2="18.379" gradientTransform="matrix(.95844 0 0 .95844 .99752 1.9975)" y1="44.98" x1="18.379">
<stop id="stop2492" style="stop-color:#791235" offset="0"/>
<stop id="stop2494" style="stop-color:#dd3b27" offset="1"/>
</linearGradient>
<radialGradient id="radialGradient2414" gradientUnits="userSpaceOnUse" cy="3.99" cx="23.896" gradientTransform="matrix(0 2.2875 -3.0194 0 36.047 -50.63)" r="20.397">
<stop id="stop3244" style="stop-color:#f8b17e" offset="0"/>
<stop id="stop3246" style="stop-color:#e35d4f" offset=".26238"/>
<stop id="stop3248" style="stop-color:#c6262e" offset=".66094"/>
<stop id="stop3250" style="stop-color:#690b54" offset="1"/>
</radialGradient>
<radialGradient id="radialGradient2419" gradientUnits="userSpaceOnUse" cy="4.625" cx="62.625" gradientTransform="matrix(2.1647 0 0 .75294 -111.56 36.518)" r="10.625">
<stop id="stop8840" offset="0"/>
<stop id="stop8842" style="stop-opacity:0" offset="1"/>
</radialGradient>
</defs>
<g id="layer1">
<g id="g3275">
<path id="path8836" style="opacity:.3;fill-rule:evenodd;fill:url(#radialGradient2419)" d="m47 40c0 4.418-10.297 8-23 8s-23-3.582-23-8 10.297-8 23-8 23 3.582 23 8z"/>
<path id="path2555" style="stroke-linejoin:round;stroke:url(#linearGradient2416);stroke-linecap:round;stroke-width:1.0037;fill:url(#radialGradient2414)" d="m24 5.5018c-10.758 0-19.498 8.7402-19.498 19.498-0.0002 10.758 8.74 19.498 19.498 19.498s19.498-8.74 19.498-19.498-8.74-19.498-19.498-19.498z"/>
<path id="path8655" style="opacity:.4;stroke:url(#linearGradient2411);fill:none" d="m42.5 24.999c0 10.218-8.283 18.501-18.5 18.501s-18.5-8.283-18.5-18.501c0-10.217 8.283-18.499 18.5-18.499s18.5 8.282 18.5 18.499z"/>
</g>
<g id="g3243" transform="translate(51.075 .56862)">
<path id="path3295" style="opacity:.2" d="m-29.451 12.554c0.563 5.5 1.208 10.961 1.687 16.482h1.53c0.397-5.302 1.038-10.571 1.501-15.867 0.236-1.254-0.408-2.742-1.732-3.047-1.308-0.3824-2.77 0.565-2.944 1.918-0.029 0.17-0.042 0.342-0.042 0.514zm-0.167 22.359c-0.059 1.637 1.742 2.92 3.28 2.401 1.489-0.38 2.274-2.252 1.51-3.583-0.683-1.375-2.687-1.829-3.84-0.776-0.582 0.479-0.968 1.194-0.95 1.958z"/>
<path id="text2315" style="fill:#fff" d="m-29.451 13.555c0.563 5.499 1.208 10.96 1.687 16.481h1.53c0.397-5.301 1.038-10.571 1.501-15.866 0.236-1.254-0.408-2.743-1.732-3.048-1.308-0.382-2.77 0.565-2.944 1.918-0.029 0.17-0.042 0.342-0.042 0.515zm-0.167 22.358c-0.059 1.637 1.742 2.921 3.28 2.402 1.489-0.381 2.274-2.253 1.51-3.584-0.683-1.375-2.687-1.828-3.84-0.776-0.582 0.479-0.968 1.194-0.95 1.958z"/>
</g>
</g>
</svg>
Side by Side

After

Width:  |  Height:  |  Size: 3.2 KiB

317
ground/openpilotgcs/src/plugins/uploader/images/warning.svg
View File

@ -1,35 +1,290 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<!-- Created with Inkscape (http://www.inkscape.org/) -->
<svg id="svg3247" xmlns="http://www.w3.org/2000/svg" height="48" width="48" version="1.0" xmlns:xlink="http://www.w3.org/1999/xlink">
<defs id="defs3249">
<linearGradient id="linearGradient2411" y2="5.4676" gradientUnits="userSpaceOnUse" x2="63.397" gradientTransform="matrix(2.1154 0 0 2.1153 -107.58 32.427)" y1="-12.489" x1="63.397">
<stop id="stop4875" style="stop-color:#fff" offset="0"/>
<stop id="stop4877" style="stop-color:#fff;stop-opacity:0" offset="1"/>
</linearGradient>
<linearGradient id="linearGradient2416" y2="3.0816" gradientUnits="userSpaceOnUse" x2="18.379" gradientTransform="matrix(.95844 0 0 .95844 .99752 1.9975)" y1="44.98" x1="18.379">
<stop id="stop2492" style="stop-color:#791235" offset="0"/>
<stop id="stop2494" style="stop-color:#dd3b27" offset="1"/>
</linearGradient>
<radialGradient id="radialGradient2414" gradientUnits="userSpaceOnUse" cy="3.99" cx="23.896" gradientTransform="matrix(0 2.2875 -3.0194 0 36.047 -50.63)" r="20.397">
<stop id="stop3244" style="stop-color:#f8b17e" offset="0"/>
<stop id="stop3246" style="stop-color:#e35d4f" offset=".26238"/>
<stop id="stop3248" style="stop-color:#c6262e" offset=".66094"/>
<stop id="stop3250" style="stop-color:#690b54" offset="1"/>
</radialGradient>
<radialGradient id="radialGradient2419" gradientUnits="userSpaceOnUse" cy="4.625" cx="62.625" gradientTransform="matrix(2.1647 0 0 .75294 -111.56 36.518)" r="10.625">
<stop id="stop8840" offset="0"/>
<stop id="stop8842" style="stop-opacity:0" offset="1"/>
</radialGradient>
</defs>
<g id="layer1">
<g id="g3275">
<path id="path8836" style="opacity:.3;fill-rule:evenodd;fill:url(#radialGradient2419)" d="m47 40c0 4.418-10.297 8-23 8s-23-3.582-23-8 10.297-8 23-8 23 3.582 23 8z"/>
<path id="path2555" style="stroke-linejoin:round;stroke:url(#linearGradient2416);stroke-linecap:round;stroke-width:1.0037;fill:url(#radialGradient2414)" d="m24 5.5018c-10.758 0-19.498 8.7402-19.498 19.498-0.0002 10.758 8.74 19.498 19.498 19.498s19.498-8.74 19.498-19.498-8.74-19.498-19.498-19.498z"/>
<path id="path8655" style="opacity:.4;stroke:url(#linearGradient2411);fill:none" d="m42.5 24.999c0 10.218-8.283 18.501-18.5 18.501s-18.5-8.283-18.5-18.501c0-10.217 8.283-18.499 18.5-18.499s18.5 8.282 18.5 18.499z"/>
<svg
xmlns:dc="http://purl.org/dc/elements/1.1/"
xmlns:cc="http://creativecommons.org/ns#"
xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#"
xmlns:svg="http://www.w3.org/2000/svg"
xmlns="http://www.w3.org/2000/svg"
xmlns:xlink="http://www.w3.org/1999/xlink"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
width="48"
height="48"
id="svg2"
version="1.1"
inkscape:version="0.48.1 "
sodipodi:docname="warning.svg">
<defs
id="defs4">
<linearGradient
inkscape:collect="always"
id="linearGradient2850">
<stop
style="stop-color:#eeeeee;stop-opacity:1;"
offset="0"
id="stop2852" />
<stop
style="stop-color:#eeeeee;stop-opacity:0;"
offset="1"
id="stop2854" />
</linearGradient>
<linearGradient
inkscape:collect="always"
id="linearGradient3657">
<stop
style="stop-color:#986601;stop-opacity:1;"
offset="0"
id="stop3659" />
<stop
id="stop3665"
offset="0.33080238"
style="stop-color:#fdca01;stop-opacity:1" />
<stop
style="stop-color:#ffff99;stop-opacity:1"
offset="1"
id="stop3661" />
</linearGradient>
<linearGradient
inkscape:collect="always"
id="linearGradient3632">
<stop
style="stop-color:#999999;stop-opacity:0"
offset="0"
id="stop3634" />
<stop
id="stop3640"
offset="0.50655138"
style="stop-color:#e6e6e6;stop-opacity:1" />
<stop
style="stop-color:#999999;stop-opacity:0;"
offset="1"
id="stop3636" />
</linearGradient>
<inkscape:perspective
sodipodi:type="inkscape:persp3d"
inkscape:vp_x="0 : 526.18109 : 1"
inkscape:vp_y="0 : 1000 : 0"
inkscape:vp_z="744.09448 : 526.18109 : 1"
inkscape:persp3d-origin="372.04724 : 350.78739 : 1"
id="perspective10" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient3632"
id="linearGradient3638"
x1="19.10717"
y1="127.34224"
x2="260.11157"
y2="127.34224"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0.19402788,0,0,0.19376669,5.5464026,989.71412)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient3632"
id="linearGradient3644"
gradientUnits="userSpaceOnUse"
x1="2.3978782"
y1="121.40089"
x2="260.11157"
y2="127.34224"
gradientTransform="matrix(-0.09701394,0.16780688,-0.16803307,-0.09688335,73.208116,1013.465)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient3632"
id="linearGradient3648"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0.09701394,0.16780688,0.16803307,-0.09688335,-8.1841884,1014.9031)"
x1="19.10717"
y1="127.34224"
x2="260.11157"
y2="127.34224" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient3657"
id="linearGradient3680"
gradientUnits="userSpaceOnUse"
x1="112.5"
y1="275.40689"
x2="112.5"
y2="-4.2200408"
gradientTransform="matrix(0.19402788,0,0,0.19376669,5.5464026,989.71412)" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient3657"
id="linearGradient2847"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0.45813096,0,0,0.45751424,0.67535737,1.90895)"
x1="112.5"
y1="275.40689"
x2="112.5"
y2="-4.2200408" />
<linearGradient
inkscape:collect="always"
xlink:href="#linearGradient2850"
id="linearGradient2856"
x1="63.993164"
y1="8.8525076"
x2="63.993164"
y2="91.242332"
gradientUnits="userSpaceOnUse"
gradientTransform="matrix(0.42352056,0,0,0.42352056,5.2603749,988.90564)" />
</defs>
<sodipodi:namedview
id="base"
pagecolor="#ffffff"
bordercolor="#666666"
borderopacity="1.0"
inkscape:pageopacity="0.0"
inkscape:pageshadow="2"
inkscape:zoom="1"
inkscape:cx="-46.02894"
inkscape:cy="-0.138668"
inkscape:document-units="px"
inkscape:current-layer="layer1"
showgrid="false"
showguides="true"
inkscape:guide-bbox="true"
inkscape:window-width="1400"
inkscape:window-height="1003"
inkscape:window-x="-4"
inkscape:window-y="-4"
inkscape:window-maximized="1"
fit-margin-top="0"
fit-margin-left="0"
fit-margin-right="0"
fit-margin-bottom="0" />
<metadata
id="metadata7">
<rdf:RDF>
<cc:Work
rdf:about="">
<dc:format>image/svg+xml</dc:format>
<dc:type
rdf:resource="http://purl.org/dc/dcmitype/StillImage" />
<dc:title></dc:title>
<cc:license
rdf:resource="" />
</cc:Work>
</rdf:RDF>
</metadata>
<g
inkscape:label="Layer 1"
inkscape:groupmode="layer"
id="layer1"
transform="translate(-9.125,-988.32372)">
<g
id="g2975"
transform="matrix(0.42352056,0,0,0.42352056,3.9898132,596.57221)">
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 15.5625,1033.9688 c 35.520833,-1e-4 71.041667,0 106.5625,0 -17.76042,-30.7084 -35.520833,-61.41672 -53.28125,-92.12505 -17.760417,30.70833 -35.520833,61.41665 -53.28125,92.12505 z"
id="path2874"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 15.03125,1034.2812 c 35.875,0 71.75,1e-4 107.625,0 -17.9375,-31.0312 -35.875,-62.06245 -53.8125,-93.0937 -17.9375,31.03125 -35.875,62.0625 -53.8125,93.0937 z"
id="path2878"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="M 68.78125,940.65625 C 50.914197,971.92452 32.089324,1003.5095 14.75,1034.5625 c 36.091043,-0.084 72.452585,0.167 108.375,-0.125 -18.12273,-31.2282 -36.179858,-62.71402 -54.34375,-93.78125 z"
id="path2882"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 68.71875,940.34375 c -18.21641,31.323 -36.515662,62.85885 -54.46875,94.28125 3.741727,0.6849 8.400562,0.01 12.437499,0.25 32.199492,-0.1047 64.634624,0.2091 96.687501,-0.1562 -2.01836,-4.5363 -5.32611,-9.2498 -7.78102,-13.8467 -15.605622,-26.8084 -31.035873,-53.9715 -46.75023,-80.5596 -0.04179,0.009 -0.102865,-0.0245 -0.125,0.0312 z"
id="path2886"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 68.625,940.0625 c -1.898442,2.49808 -3.28205,5.61534 -4.997045,8.33338 -16.533965,28.68448 -33.220463,57.33752 -49.659205,86.04162 0.03222,1.4296 2.384617,0.414 3.325,0.7187 35.335267,-0.021 70.705057,0.042 106.01875,-0.031 1.22525,-0.6532 -0.7388,-2.314 -1.00779,-3.2436 -17.6836,-30.5632 -35.377677,-61.22113 -53.05471,-91.72515 -0.17511,-0.11773 -0.421252,-0.17697 -0.625,-0.0937 z"
id="path2890"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 68.59375,939.65625 c -0.849146,0.26354 -1.046037,1.3532 -1.557052,2.02713 -17.793483,30.78262 -35.586965,61.56522 -53.380448,92.34782 -0.382416,0.719 0.174709,1.5807 0.975,1.5313 36.175,0 72.35,0 108.525,0 0.89258,-0.022 1.26685,-1.0887 0.72476,-1.7909 -18.05408,-31.2051 -36.108172,-62.41024 -54.16226,-93.61535 -0.138442,-0.39413 -0.699141,-0.63415 -1.125,-0.5 z"
id="path2894"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 68.5625,939.4375 c -1.590286,0.85105 -1.905887,3.12983 -3.033256,4.48971 -17.3826,30.12088 -34.865418,60.21889 -52.185494,90.35399 -0.26697,1.59 1.857007,1.73 2.9875,1.5 35.676932,-0.021 71.388394,0.042 107.04375,-0.031 1.49785,-0.393 0.95277,-2.2528 0.0615,-3.1109 -17.9457,-30.9718 -35.823978,-62.04135 -53.8115,-92.9518 -0.298373,-0.1963 -0.695976,-0.36722 -1.0625,-0.25 z"
id="path2898"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 68.4375,939.1875 c -2.665517,1.85677 -3.376329,5.93312 -5.441511,8.47941 -16.5968,28.90914 -33.570754,57.68499 -49.933489,86.67689 0.03767,3.0044 4.461608,1.2959 6.268751,1.7187 34.807311,-0.1049 69.688836,0.2093 104.449999,-0.1563 2.48477,-1.8446 -1.4715,-4.7803 -2.01675,-6.799 -17.3631,-29.87443 -34.529605,-59.95355 -52.0145,-89.70095 -0.363201,-0.23912 -0.899825,-0.38311 -1.3125,-0.21875 z"
id="path2902"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 68.28125,938.90625 c -4.241668,4.50666 -6.418986,11.25217 -10.039272,16.42758 -15.094516,26.42114 -30.798679,52.67447 -45.523228,79.19737 0.973486,3.9884 7.091213,0.8607 9.912501,1.8438 33.766663,-0.1709 67.711681,0.339 101.368749,-0.25 3.02785,-2.6896 -2.75748,-6.5679 -3.33688,-9.4803 -17.04167,-29.14204 -33.624067,-58.76297 -50.94437,-87.61345 -0.446728,-0.17465 -0.976209,-0.34944 -1.4375,-0.125 z"
id="path2906"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 68.25,938.625 c -4.248341,3.6207 -5.799608,10.30836 -9.257129,14.82104 -15.417424,27.05725 -31.592719,53.89936 -46.555371,81.08516 0.983203,4.5373 7.950662,1.0268 11.09375,2.125 33.518304,-0.1928 67.217157,0.3823 100.625,-0.2812 3.39168,-3.0568 -2.98383,-7.3811 -3.69406,-10.6654 C 103.54587,996.81337 87.118698,967.38512 69.90625,938.8125 69.430976,938.55208 68.767472,938.4102 68.25,938.625 z"
id="path2910"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.02999998;color:#000000;fill:#000000;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 68.15625,938.3125 c -4.44705,3.59724 -5.907996,10.41675 -9.444629,14.97729 -15.460635,27.02312 -31.4413,53.84841 -46.586621,80.99141 0.504744,5.1808 8.151862,1.69 11.40625,2.6876 33.572728,-0.2149 67.307606,0.4253 100.78125,-0.3126 3.72904,-3.2084 -2.74749,-7.7207 -3.56937,-11.1027 C 103.77,996.62017 87.401308,967.12253 70.0625,938.53125 c -0.585006,-0.29694 -1.280735,-0.43981 -1.90625,-0.21875 z"
id="path2914"
inkscape:connector-curvature="0" />
</g>
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;color:#000000;fill:#999999;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 32.613292,994.11625 a 0.38809457,0.38757213 0 0 0 -0.315295,0.19377 L 9.3056929,1034.0806 a 0.38809457,0.38757213 0 0 0 0.3334856,0.5814 l 45.9906705,0 a 0.38809457,0.38757213 0 0 0 0.333488,-0.5814 L 32.971031,994.31002 a 0.38809457,0.38757213 0 0 0 -0.357739,-0.19377 z"
id="path3618"
inkscape:connector-curvature="0" />
<path
id="path3630"
d="m 33.062615,1020.8425 -23.7569221,13.2381 c -0.1349863,0.2336 0.06345,0.5795 0.3334856,0.5814 l 45.9906705,0 c 0.270037,0 0.468473,-0.3478 0.333488,-0.5814 -22.912236,-13.1428 -22.926544,-13.255 -22.900722,-13.2381 z"
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;color:#000000;fill:url(#linearGradient3638);fill-opacity:1;stroke:none;stroke-width:4;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
sodipodi:nodetypes="cccccc"
inkscape:connector-curvature="0" />
<path
sodipodi:nodetypes="cccccc"
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;color:#000000;fill:url(#linearGradient3644);fill-opacity:1;stroke:none;stroke-width:4;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 32.854149,994.53305 c -0.135055,-0.23353 -0.534234,-0.23485 -0.670841,-0.002 L 9.1879715,1034.3063 c -0.1335054,0.2344 0.067312,0.579 0.3373558,0.5791 5.9693077,-5.4781 19.5453407,-12.2128 23.6767387,-14.9216 l -0.347917,-25.43075 z"
id="path3642"
inkscape:connector-curvature="0" />
<path
id="path3646"
d="m 32.169778,995.97114 c 0.135055,-0.23353 0.534234,-0.23485 0.670842,-0.002 l 22.995335,39.77546 c 0.133507,0.2344 -0.06731,0.5791 -0.337355,0.5791 0.102661,-0.049 -22.526131,-13.897 -22.536175,-13.8982 l -0.792647,-26.45412 z"
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;color:#000000;fill:url(#linearGradient3648);fill-opacity:1;stroke:none;stroke-width:4;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
sodipodi:nodetypes="cccccc"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;color:#000000;fill:url(#linearGradient3680);fill-opacity:1;stroke:none;stroke-width:4;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 12.313125,1032.7243 c 13.547593,0 27.095185,0 40.64278,0 -6.772788,-11.7128 -13.545573,-23.4256 -20.318359,-35.13841 -6.774808,11.71281 -13.549614,23.42561 -20.324421,35.13841 z"
id="path2838"
inkscape:connector-curvature="0" />
<path
style="fill:#000000;stroke:#333333;stroke-width:0.42352057;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 29.60586,1009.936 1.212675,13.8059 3.104446,0 1.649237,-13.8059 -5.966358,0 z"
id="path3682"
sodipodi:nodetypes="ccccc"
inkscape:connector-curvature="0" />
<path
style="fill:#000000;stroke:#333333;stroke-width:0.42352057;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1"
d="m 34.747599,1027.8352 c 0,1.3243 -1.075008,2.3979 -2.401095,2.3979 -1.326087,0 -2.401095,-1.0736 -2.401095,-2.3979 0,-1.3243 1.075008,-2.3978 2.401095,-2.3978 1.326087,0 2.401095,1.0735 2.401095,2.3978 z"
id="path3684"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.5;color:#000000;fill:#eeeeee;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 32.577451,994.12023 a 0.38809457,0.38757213 0 0 0 -0.277935,0.18529 L 9.3102902,1034.0767 a 0.38809457,0.38757213 0 0 0 0.3308755,0.5824 l 0.3176404,0 22.3407099,-38.65949 a 0.38809457,0.38757213 0 0 1 0.31764,-0.18529 0.38809457,0.38757213 0 0 1 0.357346,0.18529 l 22.340709,38.65949 0.317641,0 a 0.38809457,0.38757213 0 0 0 0.330875,-0.5824 L 32.974502,994.30552 a 0.38809457,0.38757213 0 0 0 -0.357346,-0.18529 0.38809457,0.38757213 0 0 0 -0.0397,0 z"
id="path2833"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;opacity:0.4;color:#000000;fill:#eeeeee;fill-opacity:1;stroke:none;stroke-width:1;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 9.4823455,1033.7856 -0.1720553,0.2911 a 0.38809457,0.38757213 0 0 0 0.3308755,0.5824 l 45.9916863,0 a 0.38809457,0.38757213 0 0 0 0.330875,-0.5824 l -0.172055,-0.2911 a 0.38809457,0.38757213 0 0 1 -0.15882,0.026 l -45.9916863,0 a 0.38809457,0.38757213 0 0 1 -0.1588202,-0.026 z"
id="path2839"
inkscape:connector-curvature="0" />
<path
style="font-size:medium;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-indent:0;text-align:start;text-decoration:none;line-height:normal;letter-spacing:normal;word-spacing:normal;text-transform:none;direction:ltr;block-progression:tb;writing-mode:lr-tb;text-anchor:start;color:#000000;fill:url(#linearGradient2856);fill-opacity:1;stroke:none;stroke-width:4;marker:none;visibility:visible;display:inline;overflow:visible;enable-background:accumulate;font-family:Bitstream Vera Sans;-inkscape-font-specification:Bitstream Vera Sans"
d="m 32.643626,997.58781 -12.890907,22.27449 c 4.381105,-2.304 10.146879,-3.6925 16.464362,-3.6925 2.639259,0 5.184404,0.2491 7.57043,0.7014 L 32.643626,997.58781 z"
id="path2843"
inkscape:connector-curvature="0" />
</g>
<g id="g3243" transform="translate(51.075 .56862)">
<path id="path3295" style="opacity:.2" d="m-29.451 12.554c0.563 5.5 1.208 10.961 1.687 16.482h1.53c0.397-5.302 1.038-10.571 1.501-15.867 0.236-1.254-0.408-2.742-1.732-3.047-1.308-0.3824-2.77 0.565-2.944 1.918-0.029 0.17-0.042 0.342-0.042 0.514zm-0.167 22.359c-0.059 1.637 1.742 2.92 3.28 2.401 1.489-0.38 2.274-2.252 1.51-3.583-0.683-1.375-2.687-1.829-3.84-0.776-0.582 0.479-0.968 1.194-0.95 1.958z"/>
<path id="text2315" style="fill:#fff" d="m-29.451 13.555c0.563 5.499 1.208 10.96 1.687 16.481h1.53c0.397-5.301 1.038-10.571 1.501-15.866 0.236-1.254-0.408-2.743-1.732-3.048-1.308-0.382-2.77 0.565-2.944 1.918-0.029 0.17-0.042 0.342-0.042 0.515zm-0.167 22.358c-0.059 1.637 1.742 2.921 3.28 2.402 1.489-0.381 2.274-2.253 1.51-3.584-0.683-1.375-2.687-1.828-3.84-0.776-0.582 0.479-0.968 1.194-0.95 1.958z"/>
</g>
</g>
</svg>

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 3.2 KiB

After

Width:  |  Height:  |  Size: 24 KiB

4
ground/openpilotgcs/src/plugins/uploader/runningdevicewidget.cpp
View File

@ -117,7 +117,7 @@ void runningDeviceWidget::populate()
myDevice->lblFWTag->setText(QString("Firmware tag: ")+devDesc.description);
QPixmap pix = QPixmap(QString(":uploader/images/application-certificate.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Official Firmware Build"));
myDevice->lblCertified->setToolTip(tr("Tagged officially released firmware build"));
}
else
@ -125,7 +125,7 @@ void runningDeviceWidget::populate()
myDevice->lblFWTag->setText(QString("Firmware tag: ")+devDesc.description);
QPixmap pix = QPixmap(QString(":uploader/images/warning.svg"));
myDevice->lblCertified->setPixmap(pix);
myDevice->lblCertified->setToolTip(tr("Custom Firmware Build"));
myDevice->lblCertified->setToolTip(tr("Untagged or custom firmware build"));
}
myDevice->lblGitCommitTag->setText("Git commit tag: "+devDesc.gitTag);
myDevice->lblFWDate->setText(QString("Firmware date: ") + devDesc.buildDate);

1
ground/openpilotgcs/src/plugins/uploader/uploader.qrc
View File

@ -11,5 +11,6 @@
<file>images/deviceID-0101.svg</file>
<file>images/application-certificate.svg</file>
<file>images/warning.svg</file>
<file>images/error.svg</file>
</qresource>
</RCC>

17
ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp
View File

@ -34,6 +34,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
<<"uint16_t" << "uint32_t" << "float" << "uint8_t";
QString flightObjInit,objInc,objFileNames,objNames;
qint32 sizeCalc;
flightCodePath = QDir( templatepath + QString("flight/UAVObjects"));
flightOutputPath = QDir( outputpath + QString("flight") );
flightOutputPath.mkpath(flightOutputPath.absolutePath());
@ -41,6 +42,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
flightCodeTemplate = readFile( flightCodePath.absoluteFilePath("uavobjecttemplate.c") );
flightIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjecttemplate.h") );
flightInitTemplate = readFile( flightCodePath.absoluteFilePath("uavobjectsinittemplate.c") );
flightInitIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjectsinittemplate.h") );
flightMakeTemplate = readFile( flightCodePath.absoluteFilePath("Makefiletemplate.inc") );
if ( flightCodeTemplate.isNull() || flightIncludeTemplate.isNull() || flightInitTemplate.isNull()) {
@ -48,6 +50,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
return false;
}
sizeCalc = 0;
for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) {
ObjectInfo* info=parser->getObjectByIndex(objidx);
process_object(info);
@ -57,6 +60,9 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
objInc.append("#include \"" + info->namelc + ".h\"\r\n");
objFileNames.append(" " + info->namelc);
objNames.append(" " + info->name);
if (parser->getNumBytes(objidx)>sizeCalc) {
sizeCalc = parser->getNumBytes(objidx);
}
}
// Write the flight object inialization files
@ -65,7 +71,16 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.c",
flightInitTemplate );
if (!res) {
cout << "Error: Could not write flight object init files" << endl;
cout << "Error: Could not write flight object init file" << endl;
return false;
}
// Write the flight object initialization header
flightInitIncludeTemplate.replace( QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc));
res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.h",
flightInitIncludeTemplate );
if (!res) {
cout << "Error: Could not write flight object init header file" << endl;
return false;
}

2
ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h
View File

@ -34,7 +34,7 @@ class UAVObjectGeneratorFlight
public:
bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath);
QStringList fieldTypeStrC;
QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightMakeTemplate;
QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightInitIncludeTemplate, flightMakeTemplate;
QDir flightCodePath;
QDir flightOutputPath;

2
shared/uavobjectdefinition/attitudesettings.xml
View File

@ -5,7 +5,7 @@
<field name="GyroBias" units="deg/s * 100" type="int16" elementnames="X,Y,Z" defaultvalue="0"/>
<field name="BoardRotation" units="deg" type="int16" elementnames="Roll,Pitch,Yaw" defaultvalue="0,0,0"/>
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.03"/>
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
<field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
<field name="ZeroDuringArming" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>

12
shared/uavobjectdefinition/cameradesired.xml Normal file
View File

@ -0,0 +1,12 @@
<xml>
<object name="CameraDesired" singleinstance="true" settings="false">
<description>Desired camera outputs. Comes from @ref CameraStabilization module.</description>
<field name="Roll" units="" type="float" elements="1"/>
<field name="Pitch" units="" type="float" elements="1"/>
<field name="Yaw" units="" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="never" period="0"/>
</object>
</xml>

12
shared/uavobjectdefinition/camerastabsettings.xml Normal file
View File

@ -0,0 +1,12 @@
<xml>
<object name="CameraStabSettings" singleinstance="true" settings="true">
<description>Settings for the @ref CameraStab mmodule</description>
<field name="Inputs" units="channel" type="enum" elementnames="Roll,Pitch,Yaw" options="Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5,,None" defaultvalue="None"/>
<field name="InputRange" units="deg" type="uint8" elementnames="Roll,Pitch,Yaw" defaultvalue="20"/>
<field name="OutputRange" units="deg" type="uint8" elementnames="Roll,Pitch,Yaw" defaultvalue="20"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="never" period="0"/>
</object>
</xml>

17
shared/uavobjectdefinition/hwsettings.xml
View File

@ -1,11 +1,12 @@
<xml>
<object name="HwSettings" singleinstance="true" settings="true">
<description>Selection of optional hardware configurations.</description>
<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,Spektrum,ComAux,I2C" defaultvalue="Disabled"/>
<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,S.Bus,GPS,Spektrum,ComAux" defaultvalue="Telemetry"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="never" period="0"/>
<object name="HwSettings" singleinstance="true" settings="true">
<description>Selection of optional hardware configurations.</description>
<field name="CC_FlexiPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,GPS,Spektrum,ComAux,I2C" defaultvalue="Disabled"/>
<field name="CC_MainPort" units="function" type="enum" elements="1" options="Disabled,Telemetry,S.Bus,GPS,Spektrum,ComAux" defaultvalue="Telemetry"/>
<field name="OptionalModules" units="" type="enum" elementnames="CameraStabilization,GPS" options="Disabled,Enabled" defaultvalue="Disabled"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>
<logging updatemode="never" period="0"/>
</object>
</xml>

18
shared/uavobjectdefinition/mixersettings.xml
View File

@ -5,24 +5,24 @@
<field name="FeedForward" units="" type="float" elements="1" defaultvalue="0"/>
<field name="AccelTime" units="ms" type="float" elements="1" defaultvalue="0"/>
<field name="DecelTime" units="ms" type="float" elements="1" defaultvalue="0"/>
<field name="ThrottleCurve1" units="percent" type="float" elements="5" elementnames="0,25,50,75,100" defaultvalue="0,0,0,0,0"/>
<field name="ThrottleCurve1" units="percent" type="float" elements="5" elementnames="0,25,50,75,100" defaultvalue="0,0,0,0,0"/>
<field name="Curve2Source" units="" type="enum" elements="1" options="Throttle,Roll,Pitch,Yaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Throttle"/>
<field name="ThrottleCurve2" units="percent" type="float" elements="5" elementnames="0,25,50,75,100" defaultvalue="0,0.25,0.5,0.75,1"/>
<field name="Mixer1Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer1Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer1Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
<field name="Mixer2Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer2Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer2Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
<field name="Mixer3Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer3Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer3Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
<field name="Mixer4Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer4Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer4Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
<field name="Mixer5Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer5Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer5Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
<field name="Mixer6Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer6Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer6Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
<field name="Mixer7Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer7Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer7Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
<field name="Mixer8Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer8Type" units="" type="enum" elements="1" options="Disabled,Motor,Servo,CameraRoll,CameraPitch,CameraYaw,Accessory0,Accessory1,Accessory2,Accessory3,Accessory4,Accessory5" defaultvalue="Disabled"/>
<field name="Mixer8Vector" units="" type="int8" elements="5" elementnames="ThrottleCurve1,ThrottleCurve2,Roll,Pitch,Yaw" defaultvalue="0"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>

2
shared/uavobjectdefinition/stabilizationsettings.xml
View File

@ -16,7 +16,7 @@
<field name="GyroTau" units="" type="float" elements="1" defaultvalue="0.005"/>
<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="5"/>
<field name="MaxAxisLock" units="deg" type="uint8" elements="1" defaultvalue="15"/>
<field name="MaxAxisLockRate" units="deg/s" type="uint8" elements="1" defaultvalue="2"/>
<field name="WeakLevelingKp" units="(deg/s)/deg" type="float" elements="1" defaultvalue="0.1"/>