rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity
6e2ec8aecc
LibrePilot/flight/modules/MK/MKSerial/MKSerial.c

566 lines
16 KiB
C
Raw Blame History

/**
******************************************************************************
* @addtogroup OpenPilotModules OpenPilot Modules
* @{
* @addtogroup MKSerialModule MK Serial Control Module
* @brief Connect to MK module
* @{
*
* @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 "attitudestate.h" // object that will be updated by the module
#include "positionstate.h"
#include "flightbatterystate.h"
//
// Configuration
//
#define PORT PIOS_COM_AUX
#define DEBUG_PORT PIOS_COM_GPS
#define STACK_SIZE 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
#define MAX_NB_PARS 100
// #define ENABLE_DEBUG_MSG
// #define GENERATE_BATTERY_INFO // The MK can report battery voltage, but normally the current sensor will be used, so this module should not report battery state
#if PORT == PIOS_COM_AUX
#ifndef PIOS_ENABLE_AUX_UART
#error "This module cannot be included without the AUX UART enabled"
#endif
#endif
//
// 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 MSGCMD_GET_OSD 'o'
#define MSGCMD_OSD 'O'
#define DEBUG_MSG_NICK_IDX (2 + 2 * 2)
#define DEBUG_MSG_ROLL_IDX (2 + 3 * 2)
#define OSD_MSG_CURRPOS_IDX 1
#define OSD_MSG_NB_SATS_IDX 50
#define OSD_MSG_BATT_IDX 57
#define OSD_MSG_GNDSPEED_IDX 58
#define OSD_MSG_COMPHEADING_IDX 62
#define OSD_MSG_NICK_IDX 64
#define OSD_MSG_ROLL_IDX 65
#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;
typedef struct {
float longitute;
float latitude;
float altitude;
uint8_t status;
} GpsPosition_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 Par2Int16(const MkMsg_t *msg, uint8_t index);
static int32_t Par2Int32(const MkMsg_t *msg, uint8_t index);
static int8_t Par2Int8(const MkMsg_t *msg, uint8_t index);
static void GetGpsPos(const MkMsg_t *msg, uint8_t index, GpsPosition_t *pos);
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\r", 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\r");
}
#endif /* if 0 */
static int16_t Par2Int16(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 int32_t Par2Int32(const MkMsg_t *msg, uint8_t index)
{
uint32_t val = 0;
val = (((int)msg->pars[index]) << 0) + (((int)msg->pars[index + 1]) << 8);
val += (((int)msg->pars[index + 2]) << 16) + ((int)msg->pars[index + 3] << 24);
if (val > 0xFFFFFFFF / 2) {
val -= 0xFFFFFFFF;
}
return (int32_t)val;
}
static int8_t Par2Int8(const MkMsg_t *msg, uint8_t index)
{
if (msg->pars[index] > 127) {
return msg->pars[index] - 256;
} else {
return msg->pars[index];
}
}
static void GetGpsPos(const MkMsg_t *msg, uint8_t index, GpsPosition_t *pos)
{
pos->longitute = (float)Par2Int32(msg, index) * (float)1e-7;
pos->latitude = (float)Par2Int32(msg, index + 4) * (float)1e-7;
pos->altitude = (float)Par2Int32(msg, index + 8) * (float)1e-3;
pos->status = msg->pars[index + 12];
}
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);
} 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);
}
static uint16_t VersionMsg_GetVersion(const MkMsg_t *msg)
{
return msg->pars[0] * 100 + msg->pars[1];
}
static void DoConnectedToFC(void)
{
AttitudeStateData attitudeData;
MkMsg_t msg;
DEBUG_MSG("FC\n\r");
memset(&attitudeData, 0, sizeof(attitudeData));
// Configure FC for fast reporting of the debug-message
SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_DEBUG, 10);
while (TRUE) {
if (WaitForMsg(MSGCMD_DEBUG, &msg, 500 / portTICK_RATE_MS)) {
int16_t nick;
int16_t roll;
// PrintMsg(&msg);
nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX);
roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX);
DEBUG_MSG("Att: Nick=%5d Roll=%5d\n\r", nick, roll);
attitudeData.Pitch = -(float)nick / 10;
attitudeData.Roll = -(float)roll / 10;
AttitudeStateSet(&attitudeData);
} else {
DEBUG_MSG("TO\n\r");
break;
}
}
}
static void DoConnectedToNC(void)
{
MkMsg_t msg;
GpsPosition_t pos;
AttitudeStateData attitudeData;
PositionStateData positionData;
FlightBatteryStateData flightBatteryData;
#ifdef GENERATE_BATTERY_INFO
uint8_t battStateCnt = 0;
#endif
DEBUG_MSG("NC\n\r");
memset(&attitudeData, 0, sizeof(attitudeData));
memset(&positionData, 0, sizeof(positionData));
memset(&flightBatteryData, 0, sizeof(flightBatteryData));
// Configure NC for fast reporting of the osd-message
SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_OSD, 10);
while (TRUE) {
if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS)) {
// PrintMsg(&msg);
GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos);
#if 0
DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]);
DEBUG_MSG("Nick=%d Roll=%d\n\r", Par2Int8(&msg, OSD_MSG_NICK_IDX), Par2Int8(&msg, OSD_MSG_ROLL_IDX));
DEBUG_MSG("POS #Sats=%d stat=%d lat=%d lon=%d alt=%d\n\r", msg.pars[OSD_MSG_NB_SATS_IDX], pos.status, (int)pos.latitude,
(int)pos.longitute, (int)pos.altitude);
#else
DEBUG_MSG(".");
#endif
attitudeData.Pitch = -Par2Int8(&msg, OSD_MSG_NICK_IDX);
attitudeData.Roll = -Par2Int8(&msg, OSD_MSG_ROLL_IDX);
AttitudeStateSet(&attitudeData);
positionData.Longitude = pos.longitute;
positionData.Latitude = pos.latitude;
positionData.Altitude = pos.altitude;
positionData.Satellites = msg.pars[OSD_MSG_NB_SATS_IDX];
positionData.Heading = Par2Int16(&msg, OSD_MSG_COMPHEADING_IDX);
positionData.Groundspeed = ((float)Par2Int16(&msg, OSD_MSG_GNDSPEED_IDX)) / 100 /* cm/s => m/s */;
if (positionData.Satellites < 5) {
positionData.Status = POSITIONACTUAL_STATUS_NOFIX;
} else {
positionData.Status = POSITIONACTUAL_STATUS_FIX3D;
}
PositionStateSet(&positionData);
#if GENERATE_BATTERY_INFO
if (++battStateCnt > 2) {
flightBatteryData.Voltage = (float)msg.pars[OSD_MSG_BATT_IDX] / 10;
FlightBatteryStateSet(&flightBatteryData);
battStateCnt = 0;
}
#endif
} else {
DEBUG_MSG("TO\n\r");
break;
}
}
}
static void MkSerialTask(__attribute__((unused)) void *parameters)
{
MkMsg_t msg;
uint32_t version;
bool connectionOk = FALSE;
PIOS_COM_ChangeBaud(PORT, 57600);
PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
DEBUG_MSG("MKSerial Started\n\r");
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, 250 / portTICK_RATE_MS)) {
version = VersionMsg_GetVersion(&msg);
DEBUG_MSG("%d\n\r", version);
connectionOk = TRUE;
} else {
DEBUG_MSG("TO\n\r");
}
}
// Dependent on version, decide it we are connected to NC or FC
// TODO: use slave-addr to distinguish FC/NC -> much safer
if (version < 60) {
DoConnectedToNC(); // Will only return after an error
} else {
DoConnectedToFC(); // Will only return after an error
}
connectionOk = FALSE;
vTaskDelay(250 / portTICK_RATE_MS);
}
}
/**
* 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;
}
/**
* @}
* @}
*/
Reference in New Issue View Git Blame Copy Permalink