diff --git a/flight/OpenPilot/Makefile b/flight/OpenPilot/Makefile index b6f62f87a..7bce98c47 100644 --- a/flight/OpenPilot/Makefile +++ b/flight/OpenPilot/Makefile @@ -140,6 +140,7 @@ SRC += $(OPUAVOBJ)/gcstelemetrystats.c SRC += $(OPUAVOBJ)/flighttelemetrystats.c SRC += $(OPUAVOBJ)/systemstats.c SRC += $(OPUAVOBJ)/systemalarms.c +SRC += $(OPUAVOBJ)/telemetrysettings.c ## PIOS Hardware (STM32F10x) SRC += $(PIOSSTM32F10X)/pios_sys.c diff --git a/flight/OpenPilot/Modules/Telemetry/telemetry.c b/flight/OpenPilot/Modules/Telemetry/telemetry.c index 06006ef09..7a956f3db 100644 --- a/flight/OpenPilot/Modules/Telemetry/telemetry.c +++ b/flight/OpenPilot/Modules/Telemetry/telemetry.c @@ -26,6 +26,7 @@ #include "openpilot.h" #include "flighttelemetrystats.h" #include "gcstelemetrystats.h" +#include "telemetrysettings.h" // Private constants #define MAX_QUEUE_SIZE 20 @@ -48,6 +49,7 @@ static xTaskHandle telemetryTxPriTaskHandle; static xTaskHandle telemetryRxTaskHandle; static uint32_t txErrors; static uint32_t txRetries; +static TelemetrySettingsData settings; // Private functions static void telemetryTxTask(void* parameters); @@ -61,6 +63,7 @@ static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs); static void processObjEvent(UAVObjEvent* ev); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); +static void updateSettings(); /** * Initialise the telemetry module @@ -75,8 +78,8 @@ int32_t TelemetryInitialize(void) queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); priorityQueue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent)); - // TODO: Get telemetry settings object - telemetryPort = COM_USART1; + // Get telemetry settings object + updateSettings(); // Initialise UAVTalk UAVTalkInitialize(&transmitData); @@ -88,8 +91,11 @@ int32_t TelemetryInitialize(void) txErrors = 0; txRetries = 0; memset(&ev, 0, sizeof(UAVObjEvent)); - EventPeriodicQueueCreate(&ev, queue, STATS_UPDATE_PERIOD_MS); - GCSTelemetryStatsConnectQueue(queue); + EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); + + // Listen to objects of interest + GCSTelemetryStatsConnectQueue(priorityQueue); + TelemetrySettingsConnectQueue(priorityQueue); // Start telemetry tasks xTaskCreate(telemetryTxTask, (signed char*)"TelTx", STACK_SIZE, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); @@ -189,11 +195,15 @@ static void processObjEvent(UAVObjEvent* ev) { gcsTelemetryStatsUpdated(); } + else if ( ev->obj == TelemetrySettingsHandle() ) + { + updateSettings(); + } else { // Only process event if connected to GCS or if object FlightTelemetryStats is updated FlightTelemetryStatsGet(&flightStats); - if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || UAVObjGetID(ev->obj) == FLIGHTTELEMETRYSTATS_OBJID ) + if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED || ev->obj == FlightTelemetryStatsHandle() ) { // Get object metadata UAVObjGetMetadata(ev->obj, &metadata); @@ -457,7 +467,7 @@ static void updateTelemetryStats() } else if ( flightStats.Status == FLIGHTTELEMETRYSTATS_STATUS_CONNECTED ) { - if ( gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || utalkStats.rxBytes == 0 ) + if ( gcsStats.Status != GCSTELEMETRYSTATS_STATUS_CONNECTED || utalkStats.rxObjects == 0 ) { flightStats.Status = FLIGHTTELEMETRYSTATS_STATUS_DISCONNECTED; } @@ -491,3 +501,24 @@ static void updateTelemetryStats() } } +/** + * Update the telemetry settings, called on startup and + * each time the settings object is updated + */ +static void updateSettings() +{ + // Set port + telemetryPort = COM_USART1; + // Retrieve settings + TelemetrySettingsGet(&settings); + // Set port speed + if (settings.Speed == TELEMETRYSETTINGS_SPEED_9600) + { + PIOS_COM_ChangeBaud(telemetryPort, 9600); + } + else if (settings.Speed == TELEMETRYSETTINGS_SPEED_57600) + { + PIOS_COM_ChangeBaud(telemetryPort, 57600); + } +} + diff --git a/flight/OpenPilot/UAVObjects/inc/telemetrysettings.h b/flight/OpenPilot/UAVObjects/inc/telemetrysettings.h new file mode 100644 index 000000000..068d19007 --- /dev/null +++ b/flight/OpenPilot/UAVObjects/inc/telemetrysettings.h @@ -0,0 +1,74 @@ +/** + ****************************************************************************** + * + * @file telemetrysettings.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Implementation of the TelemetrySettings object. This file has been + * automatically generated by the UAVObjectGenerator. + * + * @note Object definition file: telemetrysettings.xml. + * 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 + */ + +#ifndef TELEMETRYSETTINGS_H +#define TELEMETRYSETTINGS_H + +// Object constants +#define TELEMETRYSETTINGS_OBJID 2785592614U +#define TELEMETRYSETTINGS_NAME "TelemetrySettings" +#define TELEMETRYSETTINGS_METANAME "TelemetrySettingsMeta" +#define TELEMETRYSETTINGS_ISSINGLEINST 1 +#define TELEMETRYSETTINGS_ISSETTINGS 1 +#define TELEMETRYSETTINGS_NUMBYTES sizeof(TelemetrySettingsData) + +// Object access macros +#define TelemetrySettingsGet(dataOut) UAVObjGetData(TelemetrySettingsHandle(), dataOut) +#define TelemetrySettingsSet(dataIn) UAVObjSetData(TelemetrySettingsHandle(), dataIn) +#define TelemetrySettingsInstGet(instId, dataOut) UAVObjGetInstanceData(TelemetrySettingsHandle(), instId, dataOut) +#define TelemetrySettingsInstSet(instId, dataIn) UAVObjSetInstanceData(TelemetrySettingsHandle(), instId, dataIn) +#define TelemetrySettingsConnectQueue(queue) UAVObjConnectQueue(TelemetrySettingsHandle(), queue, EV_MASK_ALL_UPDATES) +#define TelemetrySettingsConnectCallback(cb) UAVObjConnectCallback(TelemetrySettingsHandle(), cb, EV_MASK_ALL_UPDATES) +#define TelemetrySettingsCreateInstance() UAVObjCreateInstance(TelemetrySettingsHandle()) +#define TelemetrySettingsRequestUpdate() UAVObjRequestUpdate(TelemetrySettingsHandle()) +#define TelemetrySettingsRequestInstUpdate(instId) UAVObjRequestInstanceUpdate(TelemetrySettingsHandle(), instId) +#define TelemetrySettingsUpdated() UAVObjUpdated(TelemetrySettingsHandle()) +#define TelemetrySettingsInstUpdated(instId) UAVObjUpdated(TelemetrySettingsHandle(), instId) +#define TelemetrySettingsGetMetadata(dataOut) UAVObjGetMetadata(TelemetrySettingsHandle(), dataOut) +#define TelemetrySettingsSetMetadata(dataIn) UAVObjSetMetadata(TelemetrySettingsHandle(), dataIn) + +// Object data +typedef struct { + uint8_t Speed; + +} __attribute__((packed)) TelemetrySettingsData; + +// Field information +// Field Speed information +/* Enumeration options for field Speed */ +typedef enum { TELEMETRYSETTINGS_SPEED_9600=0, TELEMETRYSETTINGS_SPEED_57600=1, } TelemetrySettingsSpeedOptions; + + +// Generic interface functions +int32_t TelemetrySettingsInitialize(); +UAVObjHandle TelemetrySettingsHandle(); + +#endif // TELEMETRYSETTINGS_H diff --git a/flight/OpenPilot/UAVObjects/telemetrysettings.c b/flight/OpenPilot/UAVObjects/telemetrysettings.c new file mode 100644 index 000000000..1ecd935b3 --- /dev/null +++ b/flight/OpenPilot/UAVObjects/telemetrysettings.c @@ -0,0 +1,101 @@ +/** + ****************************************************************************** + * + * @file telemetrysettings.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief Implementation of the TelemetrySettings object. This file has been + * automatically generated by the UAVObjectGenerator. + * + * @note Object definition file: telemetrysettings.xml. + * 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" +#include "telemetrysettings.h" + +// Private variables +static UAVObjHandle handle; + +// Private functions +static void setDefaults(UAVObjHandle obj, uint16_t instId); + +/** + * Initialize object. + * \return 0 Success + * \return -1 Failure + */ +int32_t TelemetrySettingsInitialize() +{ + // Register object with the object manager + handle = UAVObjRegister(TELEMETRYSETTINGS_OBJID, TELEMETRYSETTINGS_NAME, TELEMETRYSETTINGS_METANAME, 0, + TELEMETRYSETTINGS_ISSINGLEINST, TELEMETRYSETTINGS_ISSETTINGS, TELEMETRYSETTINGS_NUMBYTES, &setDefaults); + + // Done + if (handle != 0) + { + return 0; + } + else + { + return -1; + } +} + +/** + * Initialize object fields and metadata with the default values. + * If a default value is not specified the object fields + * will be initialized to zero. + */ +static void setDefaults(UAVObjHandle obj, uint16_t instId) +{ + TelemetrySettingsData data; + UAVObjMetadata metadata; + + // Initialize object fields to their default values + UAVObjGetInstanceData(obj, instId, &data); + memset(&data, 0, sizeof(TelemetrySettingsData)); + data.Speed = 1; + + UAVObjSetInstanceData(obj, instId, &data); + + // Initialize object metadata to their default values + metadata.telemetryAcked = 1; + metadata.telemetryUpdateMode = UPDATEMODE_ONCHANGE; + metadata.telemetryUpdatePeriod = 0; + metadata.gcsTelemetryAcked = 1; + metadata.gcsTelemetryUpdateMode = UPDATEMODE_ONCHANGE; + metadata.gcsTelemetryUpdatePeriod = 0; + metadata.loggingUpdateMode = UPDATEMODE_NEVER; + metadata.loggingUpdatePeriod = 0; + UAVObjSetMetadata(obj, &metadata); +} + +/** + * Get object handle + */ +UAVObjHandle TelemetrySettingsHandle() +{ + return handle; +} + + + diff --git a/flight/OpenPilot/UAVObjects/uavobjectsinit.c b/flight/OpenPilot/UAVObjects/uavobjectsinit.c index 7107426fa..30d31af9d 100644 --- a/flight/OpenPilot/UAVObjects/uavobjectsinit.c +++ b/flight/OpenPilot/UAVObjects/uavobjectsinit.c @@ -37,6 +37,7 @@ #include "objectpersistence.h" #include "systemalarms.h" #include "systemstats.h" +#include "telemetrysettings.h" /** @@ -54,5 +55,6 @@ void UAVObjectsInitializeAll() ObjectPersistenceInitialize(); SystemAlarmsInitialize(); SystemStatsInitialize(); + TelemetrySettingsInitialize(); } diff --git a/flight/PiOS/Common/pios_com.c b/flight/PiOS/Common/pios_com.c index 82c754521..354c1a9ee 100644 --- a/flight/PiOS/Common/pios_com.c +++ b/flight/PiOS/Common/pios_com.c @@ -63,6 +63,39 @@ int32_t PIOS_COM_Init(void) 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(COMPortTypeDef port, uint32_t baud) +{ + /* Branch depending on selected port */ + switch(port) { +#if defined(PIOS_INCLUDE_USART) + case COM_DEBUG_USART: + PIOS_USART_ChangeBaud(PIOS_COM_DEBUG_PORT, baud); + return 0; + case COM_USART1: + PIOS_USART_ChangeBaud(USART_1, baud); + return 0; + case COM_USART2: + PIOS_USART_ChangeBaud(USART_2, baud); + return 0; + case COM_USART3: + PIOS_USART_ChangeBaud(USART_3, baud); + return 0; +#endif + case COM_USB_HID: + return 0; + default: + /* Invalid port */ + return -1; + } +} + /** * Sends a package over given port * \param[in] port COM port diff --git a/flight/PiOS/STM32F10x/pios_usart.c b/flight/PiOS/STM32F10x/pios_usart.c index 833e32368..5f2511df2 100644 --- a/flight/PiOS/STM32F10x/pios_usart.c +++ b/flight/PiOS/STM32F10x/pios_usart.c @@ -182,38 +182,24 @@ void PIOS_USART_Init(void) /** * Changes the baud rate of the USART peripheral without re-initialising. -* \param[in] USARTx USART name (GPS, TELEM, AUX) -* \param[in] Baud Requested baud rate +* \param[in] usart USART name (GPS, TELEM, AUX) +* \param[in] baud Requested baud rate */ -void PIOS_USART_ChangeBaud(USART_TypeDef* USARTx, uint32_t Baud) +void PIOS_USART_ChangeBaud(USARTNumTypeDef usart, uint32_t baud) { - /* USART BRR Configuration */ - /* Configure the USART Baud Rate */ - /* Adapted from stm32f01x_usart.c */ - - uint32_t TmpReg = 0x00, ApbClock = 0x00; - uint32_t IntegerDivider = 0x00; - uint32_t FractionalDivider = 0x00; - uint32_t USARTxBase = (uint32_t)USARTx; - RCC_ClocksTypeDef RCC_ClocksStatus; - - RCC_GetClocksFreq(&RCC_ClocksStatus); - if (USARTxBase == USART1_BASE) { - ApbClock = RCC_ClocksStatus.PCLK2_Frequency; - } else { - ApbClock = RCC_ClocksStatus.PCLK1_Frequency; - } - - /* Determine the integer part */ - IntegerDivider = ((0x19 * ApbClock) / (0x04 * (Baud))); - TmpReg = (IntegerDivider / 0x64) << 0x04; - - /* Determine the fractional part */ - FractionalDivider = IntegerDivider - (0x64 * (TmpReg >> 0x04)); - TmpReg |= ((((FractionalDivider * 0x10) + 0x32) / 0x64)) & ((uint8_t)0x0F); - - /* Write to USART BRR */ - USARTx->BRR = (uint16_t)TmpReg; + USART_InitTypeDef USART_InitStructure; + USART_InitStructure.USART_WordLength = USART_WordLength_8b; + USART_InitStructure.USART_StopBits = USART_StopBits_1; + USART_InitStructure.USART_Parity = USART_Parity_No; + USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; + USART_InitStructure.USART_BaudRate = baud; + if (usart == USART_1) + USART_Init(PIOS_USART1_USART, &USART_InitStructure); + else if (usart == USART_2) + USART_Init(PIOS_USART2_USART, &USART_InitStructure); + else if (usart == USART_3) + USART_Init(PIOS_USART3_USART, &USART_InitStructure); } /** diff --git a/flight/PiOS/inc/pios_com.h b/flight/PiOS/inc/pios_com.h index 85759a7fb..60554b8e8 100644 --- a/flight/PiOS/inc/pios_com.h +++ b/flight/PiOS/inc/pios_com.h @@ -38,7 +38,7 @@ typedef enum { /* Public Functions */ extern int32_t PIOS_COM_Init(void); - +extern int32_t PIOS_COM_ChangeBaud(COMPortTypeDef port, uint32_t baud); extern int32_t PIOS_COM_SendCharNonBlocking(COMPortTypeDef port, char c); extern int32_t PIOS_COM_SendChar(COMPortTypeDef port, char c); extern int32_t PIOS_COM_SendBufferNonBlocking(COMPortTypeDef port, uint8_t *buffer, uint16_t len); diff --git a/flight/PiOS/inc/pios_usart.h b/flight/PiOS/inc/pios_usart.h index 3b8c7babc..f2734efd5 100644 --- a/flight/PiOS/inc/pios_usart.h +++ b/flight/PiOS/inc/pios_usart.h @@ -33,7 +33,7 @@ typedef enum {USART_1 = 0, USART_2 = 1, USART_3 = 2} USARTNumTypeDef; /* Public Functions */ extern void PIOS_USART_Init(void); -extern void PIOS_USART_ChangeBaud(USART_TypeDef* USARTx, uint32_t Baud); +extern void PIOS_USART_ChangeBaud(USARTNumTypeDef usart, uint32_t baud); extern int32_t PIOS_USART_RxBufferFree(USARTNumTypeDef uart); extern int32_t PIOS_USART_RxBufferUsed(USARTNumTypeDef uart);