flight/modules/MK/MKSerial/MKSerial.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup MKSerialModule MK Serial Control Module
   6  * @brief Connect to MK module
   7  * @{
   8  *
   9  * @file       MKSerial.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      Interfacing with MK via serial port
  12  * @see        The GNU Public License (GPL) Version 3
  13  *
  14  *****************************************************************************/
  15 /*
  16  * This program is free software; you can redistribute it and/or modify
  17  * it under the terms of the GNU General Public License as published by
  18  * the Free Software Foundation; either version 3 of the License, or
  19  * (at your option) any later version.
  20  *
  21  * This program is distributed in the hope that it will be useful, but
  22  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  23  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  24  * for more details.
  25  *
  26  * You should have received a copy of the GNU General Public License along
  27  * with this program; if not, write to the Free Software Foundation, Inc.,
  28  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  29  */
  30
  31 #include "openpilot.h"
  32 #include "MkSerial.h"
  33
  34 #include "attitudestate.h" // object that will be updated by the module
  35 #include "positionstate.h"
  36 #include "flightbatterystate.h"
  37
  38 //
  39 // Configuration
  40 //
  41 #define PORT          PIOS_COM_AUX
  42 #define DEBUG_PORT    PIOS_COM_GPS
  43 #define STACK_SIZE    1024
  44 #define TASK_PRIORITY (tskIDLE_PRIORITY + 3)
  45 #define MAX_NB_PARS   100
  46 // #define ENABLE_DEBUG_MSG
  47 // #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
  48
  49 #if PORT == PIOS_COM_AUX
  50 #ifndef PIOS_ENABLE_AUX_UART
  51 #error "This module cannot be included without the AUX UART enabled"
  52 #endif
  53 #endif
  54 //
  55 // Private constants
  56 //
  57 #define MSGCMD_ANY              0
  58 #define MSGCMD_GET_DEBUG        'd'
  59 #define MSGCMD_DEBUG            'D'
  60 #define MSGCMD_GET_VERSION      'v'
  61 #define MSGCMD_VERSION          'V'
  62 #define MSGCMD_GET_OSD          'o'
  63 #define MSGCMD_OSD              'O'
  64
  65 #define DEBUG_MSG_NICK_IDX      (2 + 2 * 2)
  66 #define DEBUG_MSG_ROLL_IDX      (2 + 3 * 2)
  67
  68 #define OSD_MSG_CURRPOS_IDX     1
  69 #define OSD_MSG_NB_SATS_IDX     50
  70 #define OSD_MSG_BATT_IDX        57
  71 #define OSD_MSG_GNDSPEED_IDX    58
  72 #define OSD_MSG_COMPHEADING_IDX 62
  73 #define OSD_MSG_NICK_IDX        64
  74 #define OSD_MSG_ROLL_IDX        65
  75
  76 #ifdef ENABLE_DEBUG_MSG
  77 #define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__)
  78 #else
  79 #define DEBUG_MSG(format, ...)
  80 #endif
  81
  82 //
  83 // Private types
  84 //
  85 typedef struct {
  86     uint8_t address;
  87     uint8_t cmd;
  88     uint8_t nbPars;
  89     uint8_t pars[MAX_NB_PARS];
  90 } MkMsg_t;
  91
  92 typedef struct {
  93     float   longitute;
  94     float   latitude;
  95     float   altitude;
  96     uint8_t status;
  97 } GpsPosition_t;
  98
  99 enum {
 100     MK_ADDR_ALL = 0,
 101     MK_ADDR_FC  = 1,
 102     MK_ADDR_NC  = 2,
 103     MK_ADDR_MAG = 3,
 104 };
 105
 106 //
 107 // Private variables
 108 //
 109
 110 //
 111 // Private functions
 112 //
 113 static void OnError(int line);
 114 // static void PrintMsg(const MkMsg_t* msg);
 115 static int16_t Par2Int16(const MkMsg_t *msg, uint8_t index);
 116 static int32_t Par2Int32(const MkMsg_t *msg, uint8_t index);
 117 static int8_t Par2Int8(const MkMsg_t *msg, uint8_t index);
 118 static void GetGpsPos(const MkMsg_t *msg, uint8_t index, GpsPosition_t *pos);
 119 static uint8_t WaitForBytes(uint8_t *buf, uint8_t nbBytes, portTickType xTicksToWait);
 120 static bool WaitForMsg(uint8_t cmd, MkMsg_t *msg, portTickType xTicksToWait);
 121 static void SendMsg(const MkMsg_t *msg);
 122 static void SendMsgParNone(uint8_t address, uint8_t cmd);
 123 static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0);
 124 static void MkSerialTask(void *parameters);
 125
 126 static void OnError(int line)
 127 {
 128     DEBUG_MSG("MKProcol error %d\n\r", line);
 129 }
 130
 131 #if 0
 132 static void PrintMsg(const MkMsg_t *msg)
 133 {
 134     switch (msg->address) {
 135     case MK_ADDR_ALL:
 136         DEBUG_MSG("ALL ");
 137         break;
 138     case MK_ADDR_FC:
 139         DEBUG_MSG("FC  ");
 140         break;
 141     case MK_ADDR_NC:
 142         DEBUG_MSG("NC  ");
 143         break;
 144     case MK_ADDR_MAG:
 145         DEBUG_MSG("MAG ");
 146         break;
 147     default:
 148         DEBUG_MSG("??? ");
 149         break;
 150     }
 151
 152     DEBUG_MSG("%c ", msg->cmd);
 153
 154     for (int i = 0; i < msg->nbPars; i++) {
 155         DEBUG_MSG("%02x ", msg->pars[i]);
 156     }
 157     DEBUG_MSG("\n\r");
 158 }
 159 #endif /* if 0 */
 160
 161 static int16_t Par2Int16(const MkMsg_t *msg, uint8_t index)
 162 {
 163     int16_t res;
 164
 165     res = (int)(msg->pars[index + 1]) * 256 + msg->pars[index];
 166     if (res > 0xFFFF / 2) {
 167         res -= 0xFFFF;
 168     }
 169     return res;
 170 }
 171
 172 static int32_t Par2Int32(const MkMsg_t *msg, uint8_t index)
 173 {
 174     uint32_t val = 0;
 175
 176     val  = (((int)msg->pars[index]) << 0) + (((int)msg->pars[index + 1]) << 8);
 177     val += (((int)msg->pars[index + 2]) << 16) + ((int)msg->pars[index + 3] << 24);
 178     if (val > 0xFFFFFFFF / 2) {
 179         val -= 0xFFFFFFFF;
 180     }
 181     return (int32_t)val;
 182 }
 183
 184 static int8_t Par2Int8(const MkMsg_t *msg, uint8_t index)
 185 {
 186     if (msg->pars[index] > 127) {
 187         return msg->pars[index] - 256;
 188     } else {
 189         return msg->pars[index];
 190     }
 191 }
 192
 193 static void GetGpsPos(const MkMsg_t *msg, uint8_t index, GpsPosition_t *pos)
 194 {
 195     pos->longitute = (float)Par2Int32(msg, index) * (float)1e-7;
 196     pos->latitude  = (float)Par2Int32(msg, index + 4) * (float)1e-7;
 197     pos->altitude  = (float)Par2Int32(msg, index + 8) * (float)1e-3;
 198     pos->status    = msg->pars[index + 12];
 199 }
 200
 201 static uint8_t WaitForBytes(uint8_t *buf, uint8_t nbBytes, portTickType xTicksToWait)
 202 {
 203     uint8_t nbBytesLeft = nbBytes;
 204     xTimeOutType xTimeOut;
 205
 206     vTaskSetTimeOutState(&xTimeOut);
 207
 208     // Loop until
 209     // - all bytes are received
 210     // - \r is seen
 211     // - Timeout occurs
 212     do {
 213         // Check if timeout occured
 214         if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) {
 215             break;
 216         }
 217
 218         // Check if there are some bytes
 219         if (PIOS_COM_ReceiveBufferUsed(PORT)) {
 220             *buf = PIOS_COM_ReceiveBuffer(PORT);
 221
 222             nbBytesLeft--;
 223             if (buf[0] == '\r') {
 224                 break;
 225             }
 226             buf++;
 227         } else {
 228             // Avoid tight loop
 229             // FIXME: should be blocking
 230             vTaskDelay(5);
 231         }
 232     } while (nbBytesLeft);
 233
 234     return nbBytes - nbBytesLeft;
 235 }
 236
 237 static bool WaitForMsg(uint8_t cmd, MkMsg_t *msg, portTickType xTicksToWait)
 238 {
 239     uint8_t buf[10];
 240     uint8_t n;
 241     bool done  = FALSE;
 242     bool error = FALSE;
 243     unsigned int checkVal;
 244     xTimeOutType xTimeOut;
 245
 246     vTaskSetTimeOutState(&xTimeOut);
 247
 248     while (!done && !error) {
 249         // When we are here, it means we did not encounter the message we are waiting for
 250         // Check if we did not timeout yet.
 251
 252         // Wait for start
 253         buf[0] = 0;
 254         do {
 255             if (xTaskCheckForTimeOut(&xTimeOut, &xTicksToWait)) {
 256                 return FALSE;
 257             }
 258             WaitForBytes(buf, 1, 100 / portTICK_RATE_MS);
 259         } while (buf[0] != '#');
 260
 261         // Wait for cmd and address
 262         if (WaitForBytes(buf, 2, 10 / portTICK_RATE_MS) != 2) {
 263             OnError(__LINE__);
 264             continue;
 265         }
 266         // Is this the command we are waiting for?
 267         if (cmd == 0 || cmd == buf[1]) {
 268             // OK follow this message to the end
 269             msg->address = buf[0] - 'a';
 270             msg->cmd     = buf[1];
 271
 272             checkVal     = '#' + buf[0] + buf[1];
 273
 274             // Parse parameters
 275             msg->nbPars  = 0;
 276             while (!done && !error) {
 277                 n = WaitForBytes(buf, 4, 10 / portTICK_RATE_MS);
 278                 if (n > 0 && buf[n - 1] == '\r') {
 279                     n--;
 280                     // This is the end of the message
 281                     // Get check bytes
 282                     if (n >= 2) {
 283                         unsigned int msgCeckVal;
 284                         msgCeckVal = (buf[n - 1] - '=') + (buf[n - 2] - '=') * 64;
 285                         // printf("%x %x\n", msgCeckVal, checkVal&0xFFF);
 286                         n -= 2;
 287
 288                         if (msgCeckVal == (checkVal & 0xFFF)) {
 289                             done = TRUE;
 290                         } else {
 291                             OnError(__LINE__);
 292                             error = TRUE;
 293                         }
 294                     } else {
 295                         OnError(__LINE__);
 296                         error = TRUE;
 297                     }
 298                 } else if (n == 4) {
 299                     // Parse parameters
 300                     int i;
 301                     for (i = 0; i < 4; i++) {
 302                         checkVal += buf[i];
 303                         buf[i]   -= '=';
 304                     }
 305                     if (msg->nbPars < MAX_NB_PARS) {
 306                         msg->pars[msg->nbPars] = (((buf[0] << 2) & 0xFF) | ((buf[1] >> 4)));
 307                         msg->nbPars++;
 308                     }
 309                     if (msg->nbPars < MAX_NB_PARS) {
 310                         msg->pars[msg->nbPars] = ((buf[1] & 0x0F) << 4 | (buf[2] >> 2));
 311                         msg->nbPars++;
 312                     }
 313                     if (msg->nbPars < MAX_NB_PARS) {
 314                         msg->pars[msg->nbPars] = ((buf[2] & 0x03) << 6 | buf[3]);
 315                         msg->nbPars++;
 316                     }
 317                 } else {
 318                     OnError(__LINE__);
 319                     error = TRUE;
 320                 }
 321             }
 322         }
 323     }
 324
 325     return done && !error;
 326 }
 327
 328 static void SendMsg(const MkMsg_t *msg)
 329 {
 330     uint8_t buf[10];
 331     uint16_t checkVal;
 332     uint8_t nbParsRemaining;
 333     const uint8_t *pPar;
 334
 335     // Header
 336     buf[0]   = '#';
 337     buf[1]   = msg->address + 'a';
 338     buf[2]   = msg->cmd;
 339     PIOS_COM_SendBuffer(PORT, buf, 3);
 340     checkVal = (unsigned int)'#' + buf[1] + buf[2];
 341
 342     // Parameters
 343     nbParsRemaining = msg->nbPars;
 344     pPar = msg->pars;
 345     while (nbParsRemaining) {
 346         uint8_t a, b, c;
 347
 348         a = *pPar;
 349         b = 0;
 350         c = 0;
 351
 352         nbParsRemaining--;
 353         pPar++;
 354         if (nbParsRemaining) {
 355             b = *pPar;
 356             nbParsRemaining--;
 357             pPar++;
 358             if (nbParsRemaining) {
 359                 c = *pPar;
 360                 nbParsRemaining--;
 361                 pPar++;
 362             }
 363         }
 364
 365         buf[0]    = (a >> 2) + '=';
 366         buf[1]    = (((a & 0x03) << 4) | ((b & 0xf0) >> 4)) + '=';
 367         buf[2]    = (((b & 0x0f) << 2) | ((c & 0xc0) >> 6)) + '=';
 368         buf[3]    = (c & 0x3f) + '=';
 369         checkVal += buf[0];
 370         checkVal += buf[1];
 371         checkVal += buf[2];
 372         checkVal += buf[3];
 373
 374         PIOS_COM_SendBuffer(PORT, buf, 4);
 375     }
 376
 377     checkVal &= 0xFFF;
 378     buf[0]    = (checkVal / 64) + '=';
 379     buf[1]    = (checkVal % 64) + '=';
 380     buf[2]    = '\r';
 381     PIOS_COM_SendBuffer(PORT, buf, 3);
 382 }
 383
 384 static void SendMsgParNone(uint8_t address, uint8_t cmd)
 385 {
 386     MkMsg_t msg;
 387
 388     msg.address = address;
 389     msg.cmd     = cmd;
 390     msg.nbPars  = 0;
 391
 392     SendMsg(&msg);
 393 }
 394
 395 static void SendMsgPar8(uint8_t address, uint8_t cmd, uint8_t par0)
 396 {
 397     MkMsg_t msg;
 398
 399     msg.address = address;
 400     msg.cmd     = cmd;
 401     msg.nbPars  = 1;
 402     msg.pars[0] = par0;
 403
 404     SendMsg(&msg);
 405 }
 406
 407 static uint16_t VersionMsg_GetVersion(const MkMsg_t *msg)
 408 {
 409     return msg->pars[0] * 100 + msg->pars[1];
 410 }
 411
 412 static void DoConnectedToFC(void)
 413 {
 414     AttitudeStateData attitudeData;
 415     MkMsg_t msg;
 416
 417     DEBUG_MSG("FC\n\r");
 418
 419     memset(&attitudeData, 0, sizeof(attitudeData));
 420
 421     // Configure FC for fast reporting of the debug-message
 422     SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_DEBUG, 10);
 423
 424     while (TRUE) {
 425         if (WaitForMsg(MSGCMD_DEBUG, &msg, 500 / portTICK_RATE_MS)) {
 426             int16_t nick;
 427             int16_t roll;
 428
 429             // PrintMsg(&msg);
 430             nick = Par2Int16(&msg, DEBUG_MSG_NICK_IDX);
 431             roll = Par2Int16(&msg, DEBUG_MSG_ROLL_IDX);
 432
 433             DEBUG_MSG("Att: Nick=%5d Roll=%5d\n\r", nick, roll);
 434
 435             attitudeData.Pitch = -(float)nick / 10;
 436             attitudeData.Roll  = -(float)roll / 10;
 437             AttitudeStateSet(&attitudeData);
 438         } else {
 439             DEBUG_MSG("TO\n\r");
 440             break;
 441         }
 442     }
 443 }
 444
 445 static void DoConnectedToNC(void)
 446 {
 447     MkMsg_t msg;
 448     GpsPosition_t pos;
 449     AttitudeStateData attitudeData;
 450     PositionStateData positionData;
 451     FlightBatteryStateData flightBatteryData;
 452
 453 #ifdef GENERATE_BATTERY_INFO
 454     uint8_t battStateCnt = 0;
 455 #endif
 456
 457     DEBUG_MSG("NC\n\r");
 458
 459     memset(&attitudeData, 0, sizeof(attitudeData));
 460     memset(&positionData, 0, sizeof(positionData));
 461     memset(&flightBatteryData, 0, sizeof(flightBatteryData));
 462
 463     // Configure NC for fast reporting of the osd-message
 464     SendMsgPar8(MK_ADDR_ALL, MSGCMD_GET_OSD, 10);
 465
 466     while (TRUE) {
 467         if (WaitForMsg(MSGCMD_OSD, &msg, 500 / portTICK_RATE_MS)) {
 468             // PrintMsg(&msg);
 469             GetGpsPos(&msg, OSD_MSG_CURRPOS_IDX, &pos);
 470
 471 #if 0
 472             DEBUG_MSG("Bat=%d\n\r", msg.pars[OSD_MSG_BATT_IDX]);
 473             DEBUG_MSG("Nick=%d Roll=%d\n\r", Par2Int8(&msg, OSD_MSG_NICK_IDX), Par2Int8(&msg, OSD_MSG_ROLL_IDX));
 474             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,
 475                       (int)pos.longitute, (int)pos.altitude);
 476 #else
 477             DEBUG_MSG(".");
 478 #endif
 479             attitudeData.Pitch       = -Par2Int8(&msg, OSD_MSG_NICK_IDX);
 480             attitudeData.Roll        = -Par2Int8(&msg, OSD_MSG_ROLL_IDX);
 481             AttitudeStateSet(&attitudeData);
 482
 483             positionData.Longitude   = pos.longitute;
 484             positionData.Latitude    = pos.latitude;
 485             positionData.Altitude    = pos.altitude;
 486             positionData.Satellites  = msg.pars[OSD_MSG_NB_SATS_IDX];
 487             positionData.Heading     = Par2Int16(&msg, OSD_MSG_COMPHEADING_IDX);
 488             positionData.Groundspeed = ((float)Par2Int16(&msg, OSD_MSG_GNDSPEED_IDX)) / 100 /* cm/s => m/s */;
 489             if (positionData.Satellites < 5) {
 490                 positionData.Status = POSITIONACTUAL_STATUS_NOFIX;
 491             } else {
 492                 positionData.Status = POSITIONACTUAL_STATUS_FIX3D;
 493             }
 494             PositionStateSet(&positionData);
 495
 496 #if GENERATE_BATTERY_INFO
 497             if (++battStateCnt > 2) {
 498                 flightBatteryData.Voltage = (float)msg.pars[OSD_MSG_BATT_IDX] / 10;
 499                 FlightBatteryStateSet(&flightBatteryData);
 500                 battStateCnt = 0;
 501             }
 502 #endif
 503         } else {
 504             DEBUG_MSG("TO\n\r");
 505             break;
 506         }
 507     }
 508 }
 509
 510 static void MkSerialTask(__attribute__((unused)) void *parameters)
 511 {
 512     MkMsg_t msg;
 513     uint32_t version;
 514     bool connectionOk = FALSE;
 515
 516     PIOS_COM_ChangeBaud(PORT, 57600);
 517     PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
 518
 519     DEBUG_MSG("MKSerial Started\n\r");
 520
 521     while (1) {
 522         // Wait until we get version from MK
 523         while (!connectionOk) {
 524             SendMsgParNone(MK_ADDR_ALL, MSGCMD_GET_VERSION);
 525             DEBUG_MSG("Version... ");
 526             if (WaitForMsg(MSGCMD_VERSION, &msg, 250 / portTICK_RATE_MS)) {
 527                 version = VersionMsg_GetVersion(&msg);
 528                 DEBUG_MSG("%d\n\r", version);
 529                 connectionOk = TRUE;
 530             } else {
 531                 DEBUG_MSG("TO\n\r");
 532             }
 533         }
 534
 535         // Dependent on version, decide it we are connected to NC or FC
 536         // TODO: use slave-addr to distinguish FC/NC -> much safer
 537         if (version < 60) {
 538             DoConnectedToNC(); // Will only return after an error
 539         } else {
 540             DoConnectedToFC(); // Will only return after an error
 541         }
 542
 543         connectionOk = FALSE;
 544
 545         vTaskDelay(250 / portTICK_RATE_MS);
 546     }
 547 }
 548
 549 /**
 550  * Initialise the module
 551  * \return -1 if initialisation failed
 552  * \return 0 on success
 553  */
 554 int32_t MKSerialInitialize(void)
 555 {
 556     // Start gps task
 557     xTaskCreate(MkSerialTask, (signed char *)"MkSerial", STACK_SIZE, NULL, TASK_PRIORITY, NULL);
 558
 559     return 0;
 560 }
 561
 562 /**
 563  * @}
 564  * @}
 565  */