mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-11 19:24:10 +01:00
a2d8544931
to make the flight code compile again. Needs careful review, particularly all the fixes for the signed vs unsigned comparisons. +review OPReview-459
560 lines
13 KiB
C
560 lines
13 KiB
C
/**
|
|
******************************************************************************
|
|
* @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 "attitudeactual.h" // object that will be updated by the module
|
|
#include "positionactual.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
|
|
|
|
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)
|
|
{
|
|
AttitudeActualData 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;
|
|
AttitudeActualSet(&attitudeData);
|
|
} else {
|
|
DEBUG_MSG("TO\n\r");
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
static void DoConnectedToNC(void)
|
|
{
|
|
MkMsg_t msg;
|
|
GpsPosition_t pos;
|
|
AttitudeActualData attitudeData;
|
|
PositionActualData 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);
|
|
AttitudeActualSet(&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;
|
|
}
|
|
PositionActualSet(&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;
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|