mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
Display incoming messages - untested
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@688 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
fredericg
fredericg
parent
4f4391708e
commit
0fc8240b12
@ -1,9 +1,9 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
*
|
||||
* @file GPS.c
|
||||
* @file MKSerial.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief GPS module, handles GPS and NMEA stream
|
||||
* @brief Interfacing with MK via serial port
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
@ -27,27 +27,220 @@
|
||||
#include "MkSerial.h"
|
||||
#include "gpsobject.h"
|
||||
|
||||
|
||||
// Debugging
|
||||
|
||||
|
||||
// Private functions
|
||||
static void MkSerialTask(void* parameters);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//
|
||||
// Private constants
|
||||
//
|
||||
#define PORT COM_USART1
|
||||
#define STACK_SIZE 1024
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
||||
#define MAX_NB_PARS 40
|
||||
|
||||
#define DEBUG_PORT COM_USART2
|
||||
|
||||
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
|
||||
|
||||
#define MSG_ANY 0
|
||||
|
||||
//
|
||||
// Private types
|
||||
//
|
||||
typedef struct
|
||||
{
|
||||
uint8_t address;
|
||||
uint8_t cmd;
|
||||
uint8_t nbPars;
|
||||
uint8_t pars[MAX_NB_PARS];
|
||||
} MkMsg_t;
|
||||
|
||||
enum
|
||||
{
|
||||
MK_ADDR_ALL = 0,
|
||||
MK_ADDR_FC = 1,
|
||||
MK_ADDR_NC = 2,
|
||||
MK_ADDR_MAG = 3,
|
||||
};
|
||||
|
||||
|
||||
//
|
||||
// Private variables
|
||||
//
|
||||
|
||||
//
|
||||
// Private functions
|
||||
//
|
||||
static uint8_t WaitForBytes(uint8_t* buf, uint8_t nbBytes, portTickType xTicksToWait);
|
||||
static void MkSerialTask(void* parameters);
|
||||
|
||||
static void OnError(int line)
|
||||
{
|
||||
DEBUG_MSG("MKProcol error %d\n", line);
|
||||
}
|
||||
|
||||
|
||||
void PrintMsg(const MkMsg_t* msg)
|
||||
{
|
||||
switch(msg->address)
|
||||
{
|
||||
case MK_ADDR_ALL: DEBUG_MSG("ALL "); break;
|
||||
case MK_ADDR_FC: DEBUG_MSG("FC "); break;
|
||||
case MK_ADDR_NC: DEBUG_MSG("NC "); break;
|
||||
case MK_ADDR_MAG: DEBUG_MSG("MAG "); break;
|
||||
default: DEBUG_MSG("??? "); break;
|
||||
}
|
||||
|
||||
DEBUG_MSG("%c ", msg->cmd);
|
||||
|
||||
for (int i=0; i<msg->nbPars; i++)
|
||||
{
|
||||
DEBUG_MSG("%02x ", msg->pars[i]);
|
||||
}
|
||||
DEBUG_MSG("\n");
|
||||
|
||||
}
|
||||
|
||||
|
||||
static uint8_t WaitForBytes(uint8_t* buf, uint8_t nbBytes, portTickType xTicksToWait)
|
||||
{
|
||||
uint8_t nbBytesLeft = nbBytes;
|
||||
xTimeOutType xTimeOut;
|
||||
|
||||
vTaskSetTimeOutState( &xTimeOut );
|
||||
|
||||
// Loop until
|
||||
// - all bytes are received
|
||||
// - \r is seen
|
||||
// - Timeout occurs
|
||||
do
|
||||
{
|
||||
// Check if timeout occured
|
||||
if (xTaskCheckForTimeOut( &xTimeOut, &xTicksToWait ))
|
||||
break;
|
||||
|
||||
// Check if there are some bytes
|
||||
if (PIOS_COM_ReceiveBufferUsed(PORT))
|
||||
{
|
||||
*buf = PIOS_COM_ReceiveBuffer(PORT);
|
||||
|
||||
nbBytesLeft--;
|
||||
if (buf[0] == '\r')
|
||||
break;
|
||||
buf++;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Avoid tight loop
|
||||
// FIXME: should be blocking
|
||||
vTaskDelay(5);
|
||||
}
|
||||
} while (nbBytesLeft);
|
||||
|
||||
return nbBytes - nbBytesLeft;
|
||||
}
|
||||
|
||||
bool WaitForMsg(uint8_t cmd, MkMsg_t* msg)
|
||||
{
|
||||
uint8_t buf[10];
|
||||
uint8_t n;
|
||||
bool done = FALSE;
|
||||
bool error = FALSE;
|
||||
unsigned int checkVal;
|
||||
|
||||
|
||||
while (!done && !error)
|
||||
{
|
||||
// Wait for start
|
||||
buf[0] = 0;
|
||||
do
|
||||
{
|
||||
WaitForBytes(buf, 1, 100 / portTICK_RATE_MS);
|
||||
// TODO: check to TO
|
||||
} while (buf[0] != '#');
|
||||
|
||||
// Wait for cmd and address
|
||||
if (WaitForBytes(buf, 2, 10 / portTICK_RATE_MS) != 2)
|
||||
{
|
||||
OnError(__LINE__);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Is this the command we are waiting for?
|
||||
if (cmd == 0 || cmd == buf[1])
|
||||
{
|
||||
// OK follow this message to the end
|
||||
msg->address = buf[0] - 'a';
|
||||
msg->cmd = buf[1];
|
||||
|
||||
checkVal = '#' + buf[0] + buf[1];
|
||||
|
||||
// Parse parameters
|
||||
msg->nbPars = 0;
|
||||
while(!done && !error)
|
||||
{
|
||||
n = WaitForBytes(buf, 4, 10 / portTICK_RATE_MS);
|
||||
if (n>0 && buf[n-1] == '\r')
|
||||
{
|
||||
n--;
|
||||
// This is the end of the message
|
||||
// Get check bytes
|
||||
if (n>=2)
|
||||
{
|
||||
unsigned int msgCeckVal;
|
||||
msgCeckVal = (buf[n-1]-'=') + (buf[n-2]-'=')*64;
|
||||
//printf("%x %x\n", msgCeckVal, checkVal&0xFFF);
|
||||
n -= 2;
|
||||
|
||||
if (msgCeckVal == (checkVal & 0xFFF))
|
||||
{
|
||||
done = TRUE;
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError(__LINE__);
|
||||
error = TRUE;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError(__LINE__);
|
||||
error = TRUE;
|
||||
}
|
||||
}
|
||||
else if (n==4)
|
||||
{
|
||||
// Parse parameters
|
||||
int i;
|
||||
for (i=0; i<4; i++)
|
||||
{
|
||||
checkVal += buf[i];
|
||||
buf[i] -= '=';
|
||||
}
|
||||
if (msg->nbPars < MAX_NB_PARS)
|
||||
{
|
||||
msg->pars[msg->nbPars] = (((buf[0]<<2)&0xFF) | ((buf[1] >> 4)));
|
||||
msg->nbPars++;
|
||||
}
|
||||
if (msg->nbPars < MAX_NB_PARS)
|
||||
{
|
||||
msg->pars[msg->nbPars] = ((buf[1] & 0x0F)<< 4 | (buf[2] >> 2));
|
||||
msg->nbPars++;
|
||||
}
|
||||
if (msg->nbPars < MAX_NB_PARS)
|
||||
{
|
||||
msg->pars[msg->nbPars] = ((buf[2] & 0x03)<< 6 | buf[3]);
|
||||
msg->nbPars++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
OnError(__LINE__);
|
||||
error = TRUE;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return (done && !error);
|
||||
}
|
||||
|
||||
/**
|
||||
* Initialise the module
|
||||
@ -57,38 +250,53 @@ static void MkSerialTask(void* parameters);
|
||||
int32_t MkSerialInitialize(void)
|
||||
{
|
||||
// Start gps task
|
||||
xTaskCreate(MkSerialTask, (signed char*)"MkSerial", STACK_SIZE, NULL, TASK_PRIORITY, NULL);
|
||||
xTaskCreate(MkSerialTask, (signed char*) "MkSerial", STACK_SIZE, NULL,
|
||||
TASK_PRIORITY, NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* gps task. Processes input buffer. It does not return.
|
||||
*/
|
||||
static void MkSerialTask(void* parameters)
|
||||
{
|
||||
PIOS_COM_ChangeBaud(PORT, 38400);
|
||||
PIOS_COM_SendString(PORT, "MKSerial Started\n");
|
||||
|
||||
PIOS_COM_ChangeBaud(PORT, 57600);
|
||||
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
|
||||
|
||||
DEBUG_MSG("MKSerial Started\n");
|
||||
|
||||
while(1)
|
||||
{
|
||||
int32_t len;
|
||||
|
||||
len = PIOS_COM_ReceiveBufferUsed(PORT);
|
||||
if (len)
|
||||
MkMsg_t msg;
|
||||
if (WaitForMsg(MSG_ANY, &msg))
|
||||
{
|
||||
PIOS_COM_SendFormattedString(PORT, "Received %d: ", len);
|
||||
|
||||
for (int32_t n = 0; n < len; ++n)
|
||||
{
|
||||
PIOS_COM_SendChar(PORT, PIOS_COM_ReceiveBuffer(PORT));
|
||||
}
|
||||
|
||||
PIOS_COM_SendString(PORT, "\n");
|
||||
PrintMsg(&msg);
|
||||
}
|
||||
else
|
||||
{
|
||||
DEBUG_MSG("NoMsg\n");
|
||||
}
|
||||
vTaskDelay(100 / portTICK_RATE_MS);
|
||||
}
|
||||
|
||||
// while (1)
|
||||
// {
|
||||
// int32_t len;
|
||||
//
|
||||
// len = PIOS_COM_ReceiveBufferUsed(PORT);
|
||||
// if (len)
|
||||
// {
|
||||
// PIOS_COM_SendFormattedString(PORT, "Received %d: ", len);
|
||||
//
|
||||
// for (int32_t n = 0; n < len; ++n)
|
||||
// {
|
||||
// PIOS_COM_SendChar(PORT, PIOS_COM_ReceiveBuffer(PORT));
|
||||
// }
|
||||
//
|
||||
// PIOS_COM_SendString(PORT, "\n");
|
||||
// }
|
||||
// vTaskDelay(100 / portTICK_RATE_MS);
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user