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

OP-4 Flight/Telemetry Various bug fixes while testing telemetry (working but not fully tested)

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@500 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-04-15 02:15:46 +00:00 committed by vassilis
parent 2e9bb6eadc
commit 5889849060
15 changed files with 121 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/OpenPilot/Makefile
View File

@ -108,7 +108,7 @@ DOXYGENDIR = ../Doc/Doxygen
## MODULES
SRC = $(MODEXAMPLE)/examplemodevent.c $(MODEXAMPLE)/examplemodperiodic.c $(MODEXAMPLE)/examplemodthread.c
SRC = $(MODSYSTEM)/systemmod.c
SRC += $(MODSYSTEM)/systemmod.c
SRC += $(MODTELEMETRY)/telemetry.c
SRC += $(MODGPS)/GPS.c $(MODGPS)/buffer.c

4
flight/OpenPilot/Modules/Example/examplemodevent.c
View File

@ -62,7 +62,7 @@ static void ObjectUpdatedCb(UAVObjEvent* ev);
* Initialise the module, called on startup.
* \returns 0 on success or -1 if initialisation failed
*/
int32_t ExampleModEventInitialize(void)
int32_t ExampleModEventInitialize()
{
// Listen for ExampleObject1 updates, connect a callback function
ExampleObject1ConnectCallback(&ObjectUpdatedCb);
@ -84,7 +84,7 @@ static void ObjectUpdatedCb(UAVObjEvent* ev)
{
ExampleSettingsData settings;
ExampleObject1Data data1;
ExampleObject1Data data2;
ExampleObject2Data data2;
int32_t step;
// Make sure that the object update is for ExampleObject1

18
flight/OpenPilot/Modules/Example/examplemodperiodic.c
View File

@ -27,7 +27,7 @@
/**
* Input object: ExampleSettings
* Output object: ExampleObject1
* Output object: ExampleObject2
*
* This module will periodically update the value of the ExampleObject object.
* The module settings can configure how the ExampleObject is manipulated.
@ -45,7 +45,7 @@
*/
#include "examplemodperiodic.h"
#include "exampleobject1.h" // object that will be updated by the module
#include "exampleobject2.h" // object that will be updated by the module
#include "examplesettings.h" // object holding module settings
// Private constants
@ -64,7 +64,7 @@ static void exampleTask(void* parameters);
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t ExampleModPeriodicInitialize(void)
int32_t ExampleModPeriodicInitialize()
{
// Start main task
xTaskCreate(exampleTask, (signed char*)"ExamplePeriodic", STACK_SIZE, NULL, TASK_PRIORITY, &taskHandle);
@ -78,7 +78,7 @@ int32_t ExampleModPeriodicInitialize(void)
static void exampleTask(void* parameters)
{
ExampleSettingsData settings;
ExampleObject1Data data;
ExampleObject2Data data;
int32_t step;
// Main task loop
@ -87,8 +87,13 @@ static void exampleTask(void* parameters)
// Update settings with latest value
ExampleSettingsGet(&settings);
// TODO: Remove, this is temporary for testing (force settings)
settings.StepDirection = EXAMPLESETTINGS_STEPDIRECTION_UP;
settings.StepSize = 1;
settings.UpdatePeriod = 10;
// Get the object data
ExampleObject1Get(&data);
ExampleObject2Get(&data);
// Determine how to update the data
if ( settings.StepDirection == EXAMPLESETTINGS_STEPDIRECTION_UP )
@ -107,13 +112,12 @@ static void exampleTask(void* parameters)
data.Field4[0] += step;
data.Field4[1] += step;
// Update the ExampleObject, after this function is called
// notifications to any other modules listening to that object
// will be sent and the GCS object will be updated through the
// telemetry link. All operations will take place asynchronously
// and the following call will return immediately.
ExampleObject1Set(&data);
ExampleObject2Set(&data);
// Since this module executes at fixed time intervals, we need to
// block the task until it is time for the next update.

2
flight/OpenPilot/Modules/Example/examplemodthread.c
View File

@ -66,7 +66,7 @@ static void exampleTask(void* parameters);
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t ExampleModThreadInitialize(void)
int32_t ExampleModThreadInitialize()
{
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));

2
flight/OpenPilot/Modules/Example/inc/examplemodevent.h
View File

@ -28,7 +28,7 @@
#include "openpilot.h"
int32_t ExampleModEventInitialize(void);
int32_t ExampleModEventInitialize();
#endif // EXAMPLEMODEVENT_H

2
flight/OpenPilot/Modules/Example/inc/examplemodperiodic.h
View File

@ -28,7 +28,7 @@
#include "openpilot.h"
int32_t ExampleModPeriodicInitialize(void);
int32_t ExampleModPeriodicInitialize();
#endif // EXAMPLEMODPERIODIC_H

2
flight/OpenPilot/Modules/Example/inc/examplemodthread.h
View File

@ -28,7 +28,7 @@
#include "openpilot.h"
int32_t ExampleModThreadInitialize(void);
int32_t ExampleModThreadInitialize();
#endif // EXAMPLEMODTHREAD_H

2
flight/OpenPilot/Modules/GPS/GPS.c
View File

@ -25,6 +25,8 @@
#include "openpilot.h"
#include "buffer.h"
#include "GPS.h"
#include "gpsinfo.h"
// constants/macros/typdefs

6
flight/OpenPilot/Modules/System/inc/systemmod.h
View File

@ -23,12 +23,12 @@
* with this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef SYSTEM_H
#define SYSTEM_H
#ifndef SYSTEMMOD_H
#define SYSTEMMOD_H
#include "openpilot.h"
int32_t SystemModInitialize(void);
#endif // SYSTEM_H
#endif // SYSTEMMOD_H

104
flight/OpenPilot/Modules/Telemetry/telemetry.c
View File

@ -27,9 +27,9 @@
// Private constants
#define MAX_QUEUE_SIZE 20
#define STACK_SIZE 100
#define STACK_SIZE configMINIMAL_STACK_SIZE
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define REQ_TIMEOUT_MS 500
#define REQ_TIMEOUT_MS 250
#define MAX_RETRIES 3
// Private types
@ -37,10 +37,12 @@
// Private variables
static COMPortTypeDef telemetryPort;
static xQueueHandle queue;
static xTaskHandle telemetryTaskHandle;
static xTaskHandle telemetryTxTaskHandle;
static xTaskHandle telemetryRxTaskHandle;
// Private functions
static void telemetryTask(void* parameters);
static void telemetryTxTask(void* parameters);
static void telemetryRxTask(void* parameters);
static int32_t transmitData(uint8_t* data, int32_t length);
static void registerObject(UAVObjHandle obj);
static void updateObject(UAVObjHandle obj);
@ -66,8 +68,9 @@ int32_t TelemetryInitialize(void)
// Process all registered objects and connect queue for updates
UAVObjIterate(&registerObject);
// Start telemetry task
xTaskCreate(telemetryTask, (signed char*)"Telemetry", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryTaskHandle);
// Start telemetry tasks
xTaskCreate(telemetryTxTask, (signed char*)"TelemetryTx", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryTxTaskHandle);
xTaskCreate(telemetryRxTask, (signed char*)"TelemetryRx", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryRxTaskHandle);
return 0;
}
@ -145,30 +148,29 @@ static void updateObject(UAVObjHandle obj)
}
/**
* Telemetry task. Processes queue events and periodic updates. It does not return.
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryTask(void* parameters)
static void telemetryTxTask(void* parameters)
{
UAVObjEvent ev;
UAVObjMetadata metadata;
int32_t retries;
int32_t success;
PIOS_COM_SendFormattedStringNonBlocking(telemetryPort, "sendtest");
// Loop forever
while(1)
{
// PIOS_LED_Toggle(LED2);
// Wait for queue message
if(xQueueReceive(queue, &ev, 0) == pdTRUE)
if(xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE)
{
// Get object metadata
UAVObjGetMetadata(ev.obj, &metadata);
// Act on event
retries = 0;
success = -1;
if(ev.event == EV_UPDATED || ev.event == EV_UPDATED_MANUAL)
{
// Send update to GCS (with retries)
retries = 0;
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
@ -178,7 +180,6 @@ static void telemetryTask(void* parameters)
else if(ev.event == EV_UPDATE_REQ)
{
// Request object update from GCS (with retries)
retries = 0;
while(retries < MAX_RETRIES && success == -1)
{
success = UAVTalkSendObjectRequest(ev.obj, ev.instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
@ -191,28 +192,41 @@ static void telemetryTask(void* parameters)
updateObject(UAVObjGetLinkedObj(ev.obj)); // linked object will be the actual object the metadata are for
}
}
}
}
/* This blocks the task until there is something on the buffer */
uint8_t bytes = PIOS_COM_ReceiveBufferUsed(telemetryPort);
if(bytes > 0)
/**
* Telemetry transmit task. Processes queue events and periodic updates.
*/
static void telemetryRxTask(void* parameters)
{
COMPortTypeDef inputPort;
int32_t len;
// Task loop
while (1)
{
// TODO: Disabled since the USB HID is not fully functional yet
inputPort = telemetryPort; // force input port, remove once USB HID is tested
// Determine input port (USB takes priority over telemetry port)
//if(!PIOS_USB_HID_CheckAvailable())
//{
// inputPort = telemetryPort;
//}
//else
//{
// inputPort = COM_USB_HID;
//}
// Block until data are available
// TODO: Update once the PIOS_COM is made blocking
//xSemaphoreTake(PIOS_USART1_Buffer, portMAX_DELAY);
len = PIOS_COM_ReceiveBufferUsed(inputPort);
for (int32_t n = 0; n < len; ++n)
{
PIOS_LED_Toggle(LED2);
uint8_t c=PIOS_COM_ReceiveBuffer(telemetryPort);
UAVTalkProcessInputStream(c);
//PIOS_COM_SendFormattedStringNonBlocking(COM_USART1, "%c", c);
//PIOS_LED_Toggle(LED1);
UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(inputPort));
}
else if(PIOS_COM_ReceiveBufferUsed(COM_USB_HID) > 0)
{
PIOS_LED_Toggle(LED2);
//UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(COM_USB_HID));
uint8_t c=PIOS_COM_ReceiveBuffer(COM_USB_HID);
if(c!=255)
{
PIOS_COM_SendCharNonBlocking(COM_USART1, c);
UAVTalkProcessInputStream(c);
}
}
//vTaskDelay(200 / portTICK_RATE_MS);
}
}
@ -224,19 +238,23 @@ static void telemetryTask(void* parameters)
*/
static int32_t transmitData(uint8_t* data, int32_t length)
{
COMPortTypeDef OutputPort;
COMPortTypeDef outputPort;
/* If USB HID transfer is possible */
if(!PIOS_USB_HID_CheckAvailable())
{
OutputPort = COM_USART1;
}
else
{
OutputPort = COM_USB_HID;
}
// TODO: Disabled since the USB HID is not fully functional yet
outputPort = telemetryPort; // force input port, remove once USB HID is tested
// Determine input port (USB takes priority over telemetry port)
//if(!PIOS_USB_HID_CheckAvailable())
//{
// outputPort = telemetryPort;
//}
//else
//{
// outputPort = COM_USB_HID;
//}
return PIOS_COM_SendBuffer(OutputPort, data, length);
// TODO: Update once the PIOS_COM is made blocking (it is implemented as a busy loop for now!)
//PIOS_LED_Toggle(LED2);
return PIOS_COM_SendBuffer(outputPort, data, length);
}
/**

5
flight/OpenPilot/System/inc/openpilot.h
View File

@ -31,11 +31,6 @@
/* PIOS Includes */
#include <pios.h>
/* OpenPilot Modules */
#include "telemetry.h"
#include "GPS.h"
#include "gpsinfo.h"
/* OpenPilot Includes */
#include <op_config.h>
#include <op_logging.h>

15
flight/OpenPilot/System/openpilot.c
View File

@ -27,6 +27,12 @@
/* OpenPilot Includes */
#include "openpilot.h"
#include "uavobjectsinit.h"
#include "telemetry.h"
#include "GPS.h"
#include "systemmod.h"
#include "examplemodevent.h"
#include "examplemodperiodic.h"
#include "examplemodthread.h"
/* Task Priorities */
#define PRIORITY_TASK_HOOKS (tskIDLE_PRIORITY + 3)
@ -38,7 +44,6 @@ static uint8_t sdcard_available;
FILEINFO File;
char Buffer[1024];
uint32_t Cache;
GpsInfoType GpsInfo;
/* Function Prototypes */
static void TaskTick(void *pvParameters);
@ -114,8 +119,13 @@ int main()
//xTaskCreate(TaskTesting, (signed portCHAR *)"TaskTesting", configMINIMAL_STACK_SIZE , NULL, 4, NULL);
//xTaskCreate(TaskServos, (signed portCHAR *)"Servos", configMINIMAL_STACK_SIZE , NULL, 3, NULL);
//xTaskCreate(TaskSDCard, (signed portCHAR *)"SDCard", configMINIMAL_STACK_SIZE, NULL, (tskIDLE_PRIORITY + 2), NULL);
//GpsInitialize();
/* Initialize modules */
//SystemModInitialize();
TelemetryInitialize();
//ExampleModEventInitialize();
ExampleModPeriodicInitialize();
//ExampleModThreadInitialize();
/* Start the FreeRTOS scheduler */
vTaskStartScheduler();
@ -139,7 +149,6 @@ static void TaskTick(void *pvParameters)
/* Setup the LEDs to Alternate */
PIOS_LED_On(LED1);
PIOS_LED_Off(LED2);
for(;;)
{

12
flight/OpenPilot/UAVObjects/eventdispatcher.c
View File

@ -176,7 +176,9 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue
{
if (objEntry->evInfo.cb == cb &&
objEntry->evInfo.queue == queue &&
memcmp(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent)) == 0)
objEntry->evInfo.ev.obj == ev->obj &&
objEntry->evInfo.ev.instId == ev->instId &&
objEntry->evInfo.ev.event == ev->event)
{
// Already registered, do nothing
xSemaphoreGiveRecursive(mutex);
@ -186,7 +188,9 @@ static int32_t eventPeriodicCreate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue
// Create handle
objEntry = (PeriodicObjectList*)pvPortMalloc(sizeof(PeriodicObjectList));
if (objEntry == NULL) return -1;
memcpy(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent));
objEntry->evInfo.ev.obj = ev->obj;
objEntry->evInfo.ev.instId = ev->instId;
objEntry->evInfo.ev.event = ev->event;
objEntry->evInfo.cb = cb;
objEntry->evInfo.queue = queue;
objEntry->updatePeriodMs = periodMs;
@ -216,7 +220,9 @@ static int32_t eventPeriodicUpdate(UAVObjEvent* ev, UAVObjEventCallback cb, xQue
{
if (objEntry->evInfo.cb == cb &&
objEntry->evInfo.queue == queue &&
memcmp(&objEntry->evInfo.ev, ev, sizeof(UAVObjEvent)) == 0)
objEntry->evInfo.ev.obj == ev->obj &&
objEntry->evInfo.ev.instId == ev->instId &&
objEntry->evInfo.ev.event == ev->event)
{
// Object found, update period
objEntry->updatePeriodMs = periodMs;

26
flight/OpenPilot/UAVTalk/uavtalk.c
View File

@ -71,6 +71,7 @@ int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream)
outStream = outputStream;
lock = xSemaphoreCreateRecursiveMutex();
vSemaphoreCreateBinary(respSema);
xSemaphoreTake(respSema, 0); // reset to zero
return 0;
}
@ -131,11 +132,11 @@ static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type
// Send object depending on if a response is needed
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ)
{
xSemaphoreTake(respSema, 0); // non blocking call to make sure the value is zero (binary sema)
sendObject(obj, instId, type);
respObj = obj;
respInstId = instId;
xSemaphoreGiveRecursive(lock); // need to release lock since the next call will block until a response is received
xSemaphoreTake(respSema, 0); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema)
respReceived = xSemaphoreTake(respSema, timeoutMs/portTICK_RATE_MS); // lock on object until a response is received (or timeout)
// Check if a response was received
if (respReceived == pdFALSE)
@ -194,7 +195,6 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
if (rxCount == 4)
{
// Search for object, if not found reset state machine
//objId = (tmpBuffer[3] << 24) | (tmpBuffer[2] << 16) | (tmpBuffer[1] << 8) | (tmpBuffer[0]);
objId = (tmpBuffer[0] << 24) | (tmpBuffer[1] << 16) | (tmpBuffer[2] << 8) | (tmpBuffer[3]);
obj = UAVObjGetByID(objId);
if (obj == 0)
@ -249,7 +249,7 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
tmpBuffer[rxCount++] = rxbyte;
if (rxCount == 2)
{
instId = (tmpBuffer[1] << 8) | (tmpBuffer[0]);
instId = (tmpBuffer[0] << 8) | (tmpBuffer[1]);
cs = updateChecksum(cs, tmpBuffer, 2);
rxCount = 0;
// If there is a payload get it, otherwise receive checksum
@ -289,6 +289,7 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
default:
state = STATE_SYNC;
}
// Done
return 0;
}
@ -455,10 +456,10 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
// Setup type and object id fields
objId = UAVObjGetID(obj);
txBuffer[0] = type;
txBuffer[1] = (uint8_t)(objId & 0xFF);
txBuffer[2] = (uint8_t)((objId >> 8) & 0xFF);
txBuffer[3] = (uint8_t)((objId >> 16) & 0xFF);
txBuffer[4] = (uint8_t)((objId >> 24) & 0xFF);
txBuffer[1] = (uint8_t)((objId >> 24) & 0xFF);
txBuffer[2] = (uint8_t)((objId >> 16) & 0xFF);
txBuffer[3] = (uint8_t)((objId >> 8) & 0xFF);
txBuffer[4] = (uint8_t)(objId & 0xFF);
// Setup instance ID if one is required
if (UAVObjIsSingleInstance(obj))
@ -467,8 +468,8 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
}
else
{
txBuffer[5] = (uint8_t)(instId & 0xFF);
txBuffer[6] = (uint8_t)((instId >> 8) & 0xFF);
txBuffer[5] = (uint8_t)((instId >> 8) & 0xFF);
txBuffer[6] = (uint8_t)(instId & 0xFF);
dataOffset = 7;
}
@ -500,12 +501,11 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
// Calculate checksum
cs = 0;
cs = updateChecksum(cs, txBuffer, dataOffset+length);
txBuffer[dataOffset+length] = (uint8_t)(cs & 0xFF);
txBuffer[dataOffset+length+1] = (uint8_t)((cs >> 8) & 0xFF);
txBuffer[dataOffset+length] = (uint8_t)((cs >> 8) & 0xFF);
txBuffer[dataOffset+length+1] = (uint8_t)(cs & 0xFF);
// Send buffer
if (outStream!=NULL)
(*outStream)(txBuffer, dataOffset+length+CHECKSUM_LENGTH);
if (outStream!=NULL) (*outStream)(txBuffer, dataOffset+length+CHECKSUM_LENGTH);
// Done
return 0;

5
flight/PiOS/STM32F10x/pios_usart.c
View File

@ -88,6 +88,7 @@ void PIOS_USART_Init(void)
/* Configure and Init USART Tx as alternate function open-drain */
GPIO_InitStructure.GPIO_Pin = PIOS_USART1_TX_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(PIOS_USART1_GPIO_PORT, &GPIO_InitStructure);
/* Configure and Init USART Rx input with internal pull-ups */
@ -120,7 +121,7 @@ void PIOS_USART_Init(void)
/* Configure and Init USART Tx as alternate function open-drain */
GPIO_InitStructure.GPIO_Pin = PIOS_USART2_TX_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(PIOS_USART2_GPIO_PORT, &GPIO_InitStructure);
/* Configure and Init USART Rx input with internal pull-ups */
@ -153,7 +154,7 @@ void PIOS_USART_Init(void)
/* Configure and Init USART Tx as alternate function open-drain */
GPIO_InitStructure.GPIO_Pin = PIOS_USART3_TX_PIN;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(PIOS_USART3_GPIO_PORT, &GPIO_InitStructure);
/* Configure and Init USART Rx input with internal pull-ups */