rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-11 03:29:17 +01:00
Code Issues Releases Activity

MKSerial: composing messages

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@703 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
fredericg 2010-06-02 19:44:57 +00:00 committed by fredericg
parent 4d227cd8b6
commit 59250845ea
1 changed files with 139 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
flight/OpenPilot/Modules/MK/MKSerial/MKSerial.c
View File

@ -40,7 +40,10 @@
#define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format, ## __VA_ARGS__)
#define MSG_ANY 0
#define MSG_GET_DEBUG 'd'
#define MSG_DEBUG 'D'
#define MSG_GET_VERSION 'v'
#define MSG_VERSION 'V'
//
// Private types
@ -61,7 +64,6 @@ enum
MK_ADDR_MAG = 3,
};
//
// Private variables
//
@ -77,16 +79,25 @@ 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;
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);
@ -99,7 +110,6 @@ void PrintMsg(const MkMsg_t* msg)
}
static int16_t Par2SignedInt(const MkMsg_t* msg, uint8_t index)
{
int16_t res;
@ -253,12 +263,99 @@ bool WaitForMsg(uint8_t cmd, MkMsg_t* msg)
return (done && !error);
}
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);
}
void SendMsgParNone(uint8_t address, uint8_t cmd)
{
MkMsg_t msg;
msg.address = address;
msg.cmd = cmd;
msg.nbPars = 0;
SendMsg(&msg);
}
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);
}
uint16_t VersionMsg_GetVersion(const MkMsg_t* msg)
{
return msg->pars[0] * 100 + msg->pars[1];
}
/**
* Initialise the module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t MkSerialInitialize(void)
int32_t MKSerialInitialize(void)
{
// Start gps task
xTaskCreate(MkSerialTask, (signed char*) "MkSerial", STACK_SIZE, NULL,
@ -272,12 +369,22 @@ int32_t MkSerialInitialize(void)
*/
static void MkSerialTask(void* parameters)
{
MkMsg_t msg;
PIOS_COM_ChangeBaud(PORT, 57600);
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
DEBUG_MSG("MKSerial Started\n");
SendMsgParNone(MK_ADDR_ALL, MSG_GET_VERSION);
if (WaitForMsg(MSG_GET_VERSION, &msg))
{
//PrintMsg(&msg);
DEBUG_MSG("Version = %d\n", VersionMsg_GetVersion(&msg));
}
SendMsgPar8(MK_ADDR_ALL, MSG_GET_DEBUG, 10);
while (1)
{
MkMsg_t msg;
@ -291,24 +398,4 @@ static void MkSerialTask(void* parameters)
DEBUG_MSG("NoMsg\n");
}
}
// 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);
// }
}