mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-02 10:24:11 +01:00
db629cebc5
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@717 ebee16cc-31ac-478f-84a7-5cbb03baadba
469 lines
9.7 KiB
C
469 lines
9.7 KiB
C
/**
|
|
******************************************************************************
|
|
*
|
|
* @file MKSerial.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
|
* @brief Interfacing with MK via serial port
|
|
* @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 "MkSerial.h"
|
|
|
|
#include "attitudeactual.h" // object that will be updated by the module
|
|
|
|
|
|
//
|
|
// Configuration
|
|
//
|
|
#define PORT COM_USART2
|
|
#define DEBUG_PORT COM_USART1
|
|
#define STACK_SIZE 1024
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
|
|
#define MAX_NB_PARS 40
|
|
//#define ENABLE_DEBUG_MSG
|
|
|
|
//
|
|
// Private constants
|
|
//
|
|
#define MSGCMD_ANY 0
|
|
#define MSGCMD_GET_DEBUG 'd'
|
|
#define MSGCMD_DEBUG 'D'
|
|
#define MSGCMD_GET_VERSION 'v'
|
|
#define MSGCMD_VERSION 'V'
|
|
|
|
#define DEBUG_MSG_NICK_IDX (2+2*2)
|
|
#define DEBUG_MSG_ROLL_IDX (2+3*2)
|
|
|
|
#ifdef ENABLE_DEBUG_MSG
|
|
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
|
|
#else
|
|
#define DEBUG_MSG(format, ...)
|
|
#endif
|
|
|
|
|
|
//
|
|
// 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 void OnError(int line);
|
|
//static void PrintMsg(const MkMsg_t* msg);
|
|
static int16_t Par2SignedInt(const MkMsg_t* msg, uint8_t index);
|
|
static uint8_t WaitForBytes(uint8_t* buf, uint8_t nbBytes, portTickType xTicksToWait);
|
|
static bool WaitForMsg(uint8_t cmd, MkMsg_t* msg, portTickType xTicksToWait);
|
|
static void SendMsg(const MkMsg_t* msg);
|
|
static void SendMsgParNone(uint8_t address, uint8_t cmd);
|
|
static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0);
|
|
static void MkSerialTask(void* parameters);
|
|
|
|
|
|
|
|
|
|
static void OnError(int line)
|
|
{
|
|
DEBUG_MSG("MKProcol error %d\n", line);
|
|
}
|
|
|
|
|
|
#if 0
|
|
static 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");
|
|
}
|
|
#endif
|
|
|
|
static int16_t Par2SignedInt(const MkMsg_t* msg, uint8_t index)
|
|
{
|
|
int16_t res;
|
|
|
|
res = (int) (msg->pars[index + 1]) * 256 + msg->pars[index];
|
|
if (res > 0xFFFF / 2)
|
|
res -= 0xFFFF;
|
|
return res;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
static bool WaitForMsg(uint8_t cmd, MkMsg_t* msg, portTickType xTicksToWait)
|
|
{
|
|
uint8_t buf[10];
|
|
uint8_t n;
|
|
bool done = FALSE;
|
|
bool error = FALSE;
|
|
unsigned int checkVal;
|
|
xTimeOutType xTimeOut;
|
|
|
|
|
|
vTaskSetTimeOutState(&xTimeOut);
|
|
|
|
while (!done && !error)
|
|
{
|
|
// When we are here, it means we did not encounter the message we are waiting for
|
|
// Check if we did not timeout yet.
|
|
|
|
|
|
// Wait for start
|
|
buf[0] = 0;
|
|
do
|
|
{
|
|
if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait))
|
|
{
|
|
return FALSE;
|
|
}
|
|
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);
|
|
}
|
|
|
|
static void SendMsg(const MkMsg_t* msg)
|
|
{
|
|
uint8_t buf[10];
|
|
uint16_t checkVal;
|
|
uint8_t nbParsRemaining;
|
|
const uint8_t* pPar;
|
|
|
|
// Header
|
|
buf[0] = '#';
|
|
buf[1] = msg->address + 'a';
|
|
buf[2] = msg->cmd;
|
|
PIOS_COM_SendBuffer(PORT, buf, 3);
|
|
checkVal = (unsigned int) '#' + buf[1] + buf[2];
|
|
|
|
// Parameters
|
|
nbParsRemaining = msg->nbPars;
|
|
pPar = msg->pars;
|
|
while (nbParsRemaining)
|
|
{
|
|
uint8_t a, b, c;
|
|
|
|
a = *pPar;
|
|
b = 0;
|
|
c = 0;
|
|
|
|
nbParsRemaining--;
|
|
pPar++;
|
|
if (nbParsRemaining)
|
|
{
|
|
b = *pPar;
|
|
nbParsRemaining--;
|
|
pPar++;
|
|
if (nbParsRemaining)
|
|
{
|
|
c = *pPar;
|
|
nbParsRemaining--;
|
|
pPar++;
|
|
}
|
|
}
|
|
|
|
buf[0] = (a >> 2) + '=';
|
|
buf[1] = (((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + '=';
|
|
buf[2] = (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + '=';
|
|
buf[3] = (c & 0x3f) + '=';
|
|
checkVal += buf[0];
|
|
checkVal += buf[1];
|
|
checkVal += buf[2];
|
|
checkVal += buf[3];
|
|
|
|
PIOS_COM_SendBuffer(PORT, buf, 4);
|
|
}
|
|
|
|
checkVal &= 0xFFF;
|
|
buf[0] = (checkVal / 64) + '=';
|
|
buf[1] = (checkVal % 64) + '=';
|
|
buf[2] = '\r';
|
|
PIOS_COM_SendBuffer(PORT, buf, 3);
|
|
}
|
|
|
|
static void SendMsgParNone(uint8_t address, uint8_t cmd)
|
|
{
|
|
MkMsg_t msg;
|
|
|
|
msg.address = address;
|
|
msg.cmd = cmd;
|
|
msg.nbPars = 0;
|
|
|
|
SendMsg(&msg);
|
|
}
|
|
|
|
static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0)
|
|
{
|
|
MkMsg_t msg;
|
|
|
|
msg.address = address;
|
|
msg.cmd = cmd;
|
|
msg.nbPars = 1;
|
|
msg.pars[0] = par0;
|
|
|
|
SendMsg(&msg);
|
|
}
|
|
|
|
#ifdef ENABLE_DEBUG_MSG
|
|
static uint16_t VersionMsg_GetVersion(const MkMsg_t* msg)
|
|
{
|
|
return msg->pars[0] * 100 + msg->pars[1];
|
|
}
|
|
#endif
|
|
|
|
|
|
static void MkSerialTask(void* parameters)
|
|
{
|
|
MkMsg_t msg;
|
|
AttitudeActualData data;
|
|
bool connectionOk = FALSE;
|
|
|
|
PIOS_COM_ChangeBaud(PORT, 57600);
|
|
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
|
|
|
|
memset(&data, 0, sizeof(data));
|
|
|
|
DEBUG_MSG("MKSerial Started\n");
|
|
|
|
while (1)
|
|
{
|
|
// Wait until we get version from MK
|
|
while (!connectionOk)
|
|
{
|
|
SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION);
|
|
DEBUG_MSG("Version... ");
|
|
if (WaitForMsg(MSGCMD_VERSION, &msg, 100 / portTICK_RATE_MS))
|
|
{
|
|
#ifdef ENABLE_DEBUG_MSG
|
|
uint32_t version = VersionMsg_GetVersion(&msg);
|
|
DEBUG_MSG("%d\n", version);
|
|
#endif
|
|
connectionOk = TRUE;
|
|
}
|
|
else
|
|
{
|
|
DEBUG_MSG("TO\n");
|
|
}
|
|
}
|
|
|
|
// Configure MK for fast reporting
|
|
SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_DEBUG, 10);
|
|
|
|
while (connectionOk)
|
|
{
|
|
if (WaitForMsg(MSGCMD_DEBUG, &msg, 500 / portTICK_RATE_MS))
|
|
{
|
|
int16_t nick;
|
|
int16_t roll;
|
|
|
|
//PrintMsg(&msg);
|
|
nick = Par2SignedInt(&msg, DEBUG_MSG_NICK_IDX);
|
|
roll = Par2SignedInt(&msg, DEBUG_MSG_ROLL_IDX);
|
|
|
|
DEBUG_MSG("Att: Nick=%5d Roll=%5d\n", nick, roll);
|
|
|
|
data.seq++;
|
|
data.Pitch = -(float)nick/10;
|
|
data.Roll = -(float)roll/10;
|
|
AttitudeActualSet(&data);
|
|
}
|
|
else
|
|
{
|
|
DEBUG_MSG("TO\n");
|
|
connectionOk = FALSE;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* Initialise the module
|
|
* \return -1 if initialisation failed
|
|
* \return 0 on success
|
|
*/
|
|
int32_t MKSerialInitialize(void)
|
|
{
|
|
// Start gps task
|
|
xTaskCreate(MkSerialTask, (signed char*) "MkSerial", STACK_SIZE, NULL, TASK_PRIORITY, NULL);
|
|
|
|
return 0;
|
|
}
|