flight/modules/Osd/OsdEtStd/OsdEtStd.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup OSDModule OSD Module
   6  * @brief On screen display support
   7  * @{
   8  *
   9  * @file       OsdEtStd.c
  10  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
  11  * @brief      Interfacing with EagleTree OSD Std module
  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
  32 #include "openpilot.h"
  33
  34 #include "flightbatterystate.h"
  35 #include "gpspositionsensor.h"
  36 #include "attitudestate.h"
  37 #include "barosensor.h"
  38
  39 //
  40 // Configuration
  41 //
  42 #define DEBUG_PORT PIOS_COM_GPS
  43 // #define ENABLE_DEBUG_MSG
  44 // #define PIOS_ENABLE_DEBUG_PINS
  45 // #define DUMP_CONFIG           // Enable this do read and dump the OSD config
  46
  47 //
  48 // Private constants
  49 //
  50
  51 #ifdef ENABLE_DEBUG_MSG
  52 #define DEBUG_MSG(format, ...) PIOS_COM_SendFormattedString(DEBUG_PORT, format,##__VA_ARGS__)
  53 #else
  54 #define DEBUG_MSG(format, ...)
  55 #endif
  56
  57 #define CONFIG_LENGTH             6726
  58 #define MIN(a, b) ((a) < (b) ? (a) : (b))
  59
  60 #define SUPPORTED_VERSION         115
  61
  62
  63 #define OSD_ADDRESS               0x30
  64
  65 #define OSDMSG_V_LS_IDX           10
  66 #define OSDMSG_BALT_IDX1          11
  67 #define OSDMSG_BALT_IDX2          12
  68 #define OSDMSG_A_LS_IDX           17
  69 #define OSDMSG_VA_MS_IDX          18
  70 #define OSDMSG_LAT_IDX            33
  71 #define OSDMSG_LON_IDX            37
  72 #define OSDMSG_HOME_IDX           47
  73 #define OSDMSG_ALT_IDX            49
  74 #define OSDMSG_NB_SATS            58
  75 #define OSDMSG_GPS_STAT           59
  76
  77 #define OSDMSG_GPS_STAT_NOFIX     0x03
  78 #define OSDMSG_GPS_STAT_FIX       0x2B
  79 #define OSDMSG_GPS_STAT_HB_FLAG   0x10
  80
  81 #ifdef PIOS_ENABLE_DEBUG_PINS
  82         #define DEBUG_PIN_RUNNING 0
  83         #define DEBUG_PIN_I2C     1
  84         #define DebugPinHigh(x) PIOS_DEBUG_PinHigh(x)
  85         #define DebugPinLow(x)  PIOS_DEBUG_PinLow(x)
  86 #else
  87         #define DebugPinHigh(x)
  88         #define DebugPinLow(x)
  89 #endif
  90
  91
  92 static const char *UpdateConfFilePath = "/etosd/update.ocf";
  93 #ifdef DUMP_CONFIG
  94 static const char *DumpConfFilePath   = "/etosd/dump.ocf";
  95 #endif
  96
  97 //
  98 // Private types
  99 //
 100
 101 //
 102 // Private variables
 103 //
 104
 105 // | Header / cmd?|                                  | V  |      E3                          |  V |                                                                     | LAT: 37 57.0000   | LONG: 24 00.4590  |                             | Hom<-179| Alt 545.4 m       |                        |#sat|stat|
 106 // 00   01   02   03   04   05   06   07   08   09   10   11   12   13   14   15   16   17   18   19   20   21   22   23   24   25   26   27   28   29   30   31   32   33   34   35   36   37   38   39   40   41   42   43   44   45   46   47   48   49   50   51   52   53   54   55   56   57   58   59   60   61   62
 107 uint8_t msg[63] =
 108 { 0x03, 0x3F, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x90, 0x0A, 0x00, 0xE4, 0x30, 0x00, 0x00, 0x00, 0x00, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 109   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x60, 0x10, 0x02, 0x00, 0x00, 0x90,0x00,
 110   0x54, 0x54, 0x00, 0x00, 0x33, 0x28, 0x13, 0x00, 0x00, 0x08, 0x00, 0x00, 0x90, 0x0A };
 111 static volatile bool newPosData  = FALSE;
 112 static volatile bool newBattData = FALSE;
 113 static volatile bool newBaroData = FALSE;
 114
 115 static enum {
 116     STATE_DETECT,
 117     STATE_UPDATE_CONF,
 118     STATE_DUMP_CONF,
 119     STTE_RUNNING
 120 } state;
 121
 122 static UAVObjEvent ev;
 123 static uint32_t version = 0;
 124
 125
 126 //
 127 // Private functions
 128 //
 129 static void WriteToMsg8(uint8_t index, uint8_t value)
 130 {
 131     if (value > 100) {
 132         value = 100;
 133     }
 134
 135     msg[index] = ((value / 10) << 4) + (value % 10);
 136 }
 137
 138 static void WriteToMsg16(uint8_t index, uint16_t value)
 139 {
 140     WriteToMsg8(index, value % 100);
 141     WriteToMsg8(index + 1, value / 100);
 142 }
 143
 144 static void WriteToMsg24(uint8_t index, uint32_t value)
 145 {
 146     WriteToMsg16(index, value % 10000);
 147     WriteToMsg8(index + 2, value / 10000);
 148 }
 149
 150 static void WriteToMsg32(uint8_t index, uint32_t value)
 151 {
 152     WriteToMsg16(index, value % 10000);
 153     WriteToMsg16(index + 2, value / 10000);
 154 }
 155
 156 static void SetCoord(uint8_t index, uint32_t coord)
 157 {
 158 #define E7 10000000
 159     uint8_t deg = coord / E7;
 160     float sec   = (float)(coord - deg * E7) / ((float)E7 / (60.0 * 10000));
 161
 162     WriteToMsg8(index + 3, deg);
 163     WriteToMsg24(index, sec);
 164 }
 165
 166 static void SetCourse(uint16_t dir)
 167 {
 168     WriteToMsg16(OSDMSG_HOME_IDX, dir);
 169 }
 170
 171 static void SetBaroSensor(int16_t altitudeMeter)
 172 {
 173     // calculated formula
 174     // ET OSD uses first update as zeropoint and then +- from that
 175     altitudeMeter = (4571 - altitudeMeter) / 0.37;
 176     msg[OSDMSG_BALT_IDX1] = (uint8_t)(altitudeMeter & 0x00FF);
 177     msg[OSDMSG_BALT_IDX2] = (altitudeMeter >> 8) & 0x3F;
 178 }
 179
 180
 181 static void SetAltitude(uint32_t altitudeMeter)
 182 {
 183     WriteToMsg32(OSDMSG_ALT_IDX, altitudeMeter * 10);
 184 }
 185
 186 static void SetVoltage(uint32_t milliVolt)
 187 {
 188     msg[OSDMSG_VA_MS_IDX] &= 0x0F;
 189     msg[OSDMSG_VA_MS_IDX] |= (milliVolt / 6444) << 4;
 190     msg[OSDMSG_V_LS_IDX]   = (milliVolt % 6444) * 256 / 6444;
 191 }
 192
 193 static void SetCurrent(uint32_t milliAmp)
 194 {
 195     uint32_t value = (milliAmp * 16570 / 1000000) + 0x7FA;
 196
 197     msg[OSDMSG_VA_MS_IDX] &= 0xF0;
 198     msg[OSDMSG_VA_MS_IDX] |= ((value >> 8) & 0x0F);
 199     msg[OSDMSG_A_LS_IDX]   = (value & 0xFF);
 200 }
 201
 202 static void SetNbSats(uint8_t nb)
 203 {
 204     msg[OSDMSG_NB_SATS] = nb;
 205 }
 206
 207 static void FlightBatteryStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 208 {
 209     newBattData = TRUE;
 210 }
 211
 212 static void GPSPositionSensorUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 213 {
 214     newPosData = TRUE;
 215 }
 216
 217 static void BaroSensorUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 218 {
 219     newBaroData = TRUE;
 220 }
 221
 222 static bool Read(uint32_t start, uint8_t length, uint8_t *buffer)
 223 {
 224     uint8_t cmd[5];
 225
 226     const struct pios_i2c_txn txn_list[] = {
 227         {
 228             .addr = OSD_ADDRESS,
 229             .rw   = PIOS_I2C_TXN_WRITE,
 230             .len  = sizeof(cmd),
 231             .buf  = cmd,
 232         }
 233         ,
 234         {
 235             .addr = OSD_ADDRESS,
 236             .rw   = PIOS_I2C_TXN_READ,
 237             .len  = length,
 238             .buf  = buffer,
 239         }
 240         ,
 241     };
 242
 243     cmd[0] = 0x02;
 244     cmd[1] = 0x05;
 245     cmd[2] = (uint8_t)(start & 0xFF);
 246     cmd[3] = (uint8_t)(start >> 8);
 247     cmd[4] = length;
 248
 249     return PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list)) == 0;
 250 }
 251
 252 static bool Write(uint32_t start, uint8_t length, const uint8_t *buffer)
 253 {
 254     uint8_t cmd[125];
 255     uint8_t ack[3];
 256
 257     const struct pios_i2c_txn txn_list1[] = {
 258         {
 259             .addr = OSD_ADDRESS,
 260             .rw   = PIOS_I2C_TXN_WRITE,
 261             .len  = sizeof(cmd),
 262             .buf  = cmd,
 263         }
 264         ,
 265         {
 266             .addr = OSD_ADDRESS,
 267             .rw   = PIOS_I2C_TXN_READ,
 268             .len  = sizeof(ack),
 269             .buf  = ack,
 270         }
 271         ,
 272     };
 273
 274     if (length + 5 > sizeof(cmd)) {
 275         // Too big
 276         return FALSE;
 277     }
 278
 279     cmd[0] = 0x01;
 280     cmd[1] = 0x7D;
 281     cmd[2] = (uint8_t)(start & 0xFF);
 282     cmd[3] = (uint8_t)(start >> 8);
 283     cmd[4] = length;
 284     memcpy(&cmd[5], buffer, length);
 285
 286     ack[0] = 0;
 287
 288     //
 289     // FIXME: See OP-305, the driver seems to return FALSE while all is OK
 290     //
 291     PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1));
 292 // if (PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list1, NELEMENTS(txn_list1))) {
 293     // DEBUG_MSG("ACK=%d ", ack[0]);
 294     if (ack[0] == 49) {
 295         return TRUE;
 296     }
 297 // }
 298
 299     return FALSE;
 300 }
 301
 302 static uint32_t ReadSwVersion(void)
 303 {
 304     uint8_t buf[4];
 305     uint32_t version;
 306
 307     if (Read(0, 4, buf)) {
 308         version  = (buf[0] - '0') * 100;
 309         version += (buf[2] - '0') * 10;
 310         version += (buf[3] - '0');
 311     } else {
 312         version = 0;
 313     }
 314
 315     return version;
 316 }
 317
 318 static void UpdateConfig(void)
 319 {
 320     uint8_t buf[120];
 321     uint32_t addr = 0;
 322     uint32_t n;
 323     FILEINFO file;
 324     uint32_t res;
 325
 326     // Try to open the file that contains a new config
 327     res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)UpdateConfFilePath, DFS_READ, PIOS_SDCARD_Sector, &file);
 328     if (res == DFS_OK) {
 329         uint32_t bytesRead;
 330         bool ok = TRUE;
 331
 332         DEBUG_MSG("Updating Config ");
 333
 334         // Write the config-data in blocks to OSD
 335         while (addr < CONFIG_LENGTH && ok) {
 336             n   = MIN(CONFIG_LENGTH - addr, sizeof(buf));
 337             res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf, &bytesRead, n);
 338             if (res == DFS_OK && bytesRead == n) {
 339                 ok = Write(addr, n, buf);
 340                 if (ok) {
 341                     // DEBUG_MSG(" %d %d\n", addr, n);
 342                     DEBUG_MSG(".");
 343                     addr += n;
 344                 }
 345             } else {
 346                 DEBUG_MSG(" FILEREAD FAILED ");
 347             }
 348         }
 349
 350         DEBUG_MSG(ok ? " OK\n" : "FAILURE\n");
 351
 352         // If writing is OK, read the data back and verify
 353         if (ok) {
 354             DEBUG_MSG("Verify Config ");
 355             DFS_Seek(&file, 0, PIOS_SDCARD_Sector);
 356
 357             addr = 0;
 358             while (addr < CONFIG_LENGTH && ok) {
 359                 // First half of the buffer is used to store the data read from the OSD, the second half will contain the data from the file
 360                 n  = MIN(CONFIG_LENGTH - addr, sizeof(buf) / 2);
 361                 ok = Read(addr, n, buf);
 362                 if (ok) {
 363                     uint32_t bytesRead;
 364                     res = DFS_ReadFile(&file, PIOS_SDCARD_Sector, buf + sizeof(buf) / 2, &bytesRead, n);
 365                     if (res == DFS_OK && bytesRead == n) {
 366                         DEBUG_MSG(".");
 367                         addr += n;
 368
 369                         if (memcmp(buf, buf + sizeof(buf) / 2, n) != 0) {
 370                             DEBUG_MSG(" MISMATCH ");
 371                             ok = FALSE;
 372                         }
 373                     }
 374                 }
 375             }
 376             DEBUG_MSG(ok ? " OK\n" : "FAILURE\n");
 377         }
 378
 379         DFS_Close(&file);
 380
 381         // When the config was updated correctly, remove the config-file
 382         if (ok) {
 383             DFS_UnlinkFile(&PIOS_SDCARD_VolInfo, (uint8_t *)UpdateConfFilePath, PIOS_SDCARD_Sector);
 384         }
 385     }
 386 }
 387
 388 static void DumpConfig(void)
 389 {
 390 #ifdef DUMP_CONFIG
 391     uint8_t buf[100];
 392     uint32_t addr = 0;
 393     uint32_t n;
 394     FILEINFO file;
 395     uint32_t res;
 396
 397     DEBUG_MSG("Dumping Config ");
 398
 399     res = DFS_OpenFile(&PIOS_SDCARD_VolInfo, (uint8_t *)DumpConfFilePath, DFS_WRITE, PIOS_SDCARD_Sector, &file);
 400     if (res == DFS_OK) {
 401         uint32_t bytesWritten;
 402         bool ok = TRUE;
 403
 404         while (addr < CONFIG_LENGTH && ok) {
 405             n  = MIN(CONFIG_LENGTH - addr, sizeof(buf));
 406             ok = Read(addr, n, buf);
 407             if (ok) {
 408                 res = DFS_WriteFile(&file, PIOS_SDCARD_Sector, buf, &bytesWritten, n);
 409                 if (res == DFS_OK && bytesWritten == n) {
 410                     // DEBUG_MSG(" %d %d\n", addr, n);
 411                     DEBUG_MSG(".");
 412                     addr += n;
 413                 }
 414             }
 415         }
 416
 417         DEBUG_MSG(ok ? " OK\n" : "FAILURE\n");
 418
 419         DFS_Close(&file);
 420     } else {
 421         DEBUG_MSG("Error Opening File %x\n", res);
 422     }
 423 #endif /* ifdef DUMP_CONFIG */
 424 }
 425
 426 static void Run(void)
 427 {
 428     static uint32_t cnt = 0;
 429
 430     if (newBattData) {
 431         FlightBatteryStateData flightBatteryData;
 432
 433         FlightBatteryStateGet(&flightBatteryData);
 434
 435         // DEBUG_MSG("%5d Batt: V=%dmV\n\r", cnt, (uint32_t)(flightBatteryData.Voltage*1000));
 436
 437         SetVoltage((uint32_t)(flightBatteryData.Voltage * 1000));
 438         SetCurrent((uint32_t)(flightBatteryData.Current * 1000));
 439         newBattData = FALSE;
 440     }
 441
 442     if (newPosData) {
 443         GPSPositionSensorData positionData;
 444         AttitudeStateData attitudeStateData;
 445
 446         GPSPositionSensorGet(&positionData);
 447         AttitudeStateGet(&attitudeStateData);
 448
 449         // DEBUG_MSG("%5d Pos: #stat=%d #sats=%d alt=%d\n\r", cnt,
 450         // positionData.Status, positionData.Satellites, (uint32_t)positionData.Altitude);
 451
 452         // GPS Status
 453         if (positionData.Status == GPSPOSITIONSENSOR_STATUS_FIX3D) {
 454             msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_FIX;
 455         } else {
 456             msg[OSDMSG_GPS_STAT] = OSDMSG_GPS_STAT_NOFIX;
 457         }
 458         msg[OSDMSG_GPS_STAT] |= OSDMSG_GPS_STAT_HB_FLAG;
 459
 460         // GPS info
 461         SetCoord(OSDMSG_LAT_IDX, positionData.Latitude);
 462         SetCoord(OSDMSG_LON_IDX, positionData.Longitude);
 463         SetAltitude(positionData.Altitude);
 464         SetNbSats(positionData.Satellites);
 465         SetCourse(attitudeStateData.Yaw);
 466
 467         newPosData = FALSE;
 468     } else {
 469         msg[OSDMSG_GPS_STAT] &= ~OSDMSG_GPS_STAT_HB_FLAG;
 470     }
 471     if (newBaroData) {
 472         BaroSensorData baroData;
 473
 474         BaroSensorGet(&baroData);
 475         SetBaroSensor(baroData.Altitude);
 476
 477         newBaroData = FALSE;
 478     }
 479
 480     DEBUG_MSG("SendMsg %d\n", cnt);
 481     {
 482         DebugPinHigh(DEBUG_PIN_I2C);
 483         const struct pios_i2c_txn txn_list[] = {
 484             {
 485                 .addr = OSD_ADDRESS,
 486                 .rw   = PIOS_I2C_TXN_WRITE,
 487                 .len  = sizeof(msg),
 488                 .buf  = msg,
 489             }
 490             ,
 491         };
 492
 493         PIOS_I2C_Transfer(PIOS_I2C_MAIN_ADAPTER, txn_list, NELEMENTS(txn_list));
 494         DebugPinLow(DEBUG_PIN_I2C);
 495     }
 496
 497     cnt++;
 498 }
 499
 500 static void onTimer(UAVObjEvent *ev)
 501 {
 502     DebugPinHigh(DEBUG_PIN_RUNNING);
 503
 504         #ifdef ENABLE_DEBUG_MSG
 505     PIOS_COM_ChangeBaud(DEBUG_PORT, 57600);
 506         #endif
 507
 508     if (state == STATE_DETECT) {
 509         version = ReadSwVersion();
 510         DEBUG_MSG("SW: %d ", version);
 511
 512         if (version == SUPPORTED_VERSION) {
 513             DEBUG_MSG("OK\n");
 514             state++;
 515         } else {
 516             DEBUG_MSG("INVALID\n");
 517         }
 518     } else if (state == STATE_UPDATE_CONF) {
 519         UpdateConfig();
 520         state++;
 521     } else if (state == STATE_DUMP_CONF) {
 522         DumpConfig();
 523         state++;
 524     } else if (state == STTE_RUNNING) {
 525         Run();
 526     } else {
 527         // should not happen..
 528         state = STATE_DETECT;
 529     }
 530
 531     DebugPinLow(DEBUG_PIN_RUNNING);
 532 }
 533
 534 //
 535 // Public functions
 536 //
 537
 538
 539 /**
 540  * Initialise the module
 541  * \return -1 if initialisation failed
 542  * \return 0 on success
 543  */
 544 int32_t OsdEtStdInitialize(void)
 545 {
 546     GPSPositionSensorConnectCallback(GPSPositionSensorUpdatedCb);
 547     FlightBatteryStateConnectCallback(FlightBatteryStateUpdatedCb);
 548     BaroSensorConnectCallback(BaroSensorUpdatedCb);
 549
 550     memset(&ev, 0, sizeof(UAVObjEvent));
 551     EventPeriodicCallbackCreate(&ev, onTimer, 100 / portTICK_RATE_MS);
 552
 553     return 0;
 554 }