/branches/full-calibration/flight/OpenPilot/Modules/MK/MKSerial/MKSerial.c
C | 559 lines | 396 code | 78 blank | 85 comment | 60 complexity | 24db6e87a775b2c8d57172cf9590158f MD5 | raw file
- /**
- ******************************************************************************
- * @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(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;
- }
- /**
- * @}
- * @}
- */