FRSKY telemetry integration into Multiwii 2.4 has started

This forum is dedicated to software development related to MultiWii.
It is not the right place to submit a setup problem.
Software download
gandalf33
Posts: 10
Joined: Thu Jul 02, 2015 9:12 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by gandalf33 »

Hello Quadbow

I agree with you the value after point is mostly useless, but the issue was that before you where only sending 0x10 part, and that was not recognized anymore as a valid sensors, once I started sending the 0x21 part as well, then the sensors appeared. The voltage meseaure is done on the controler board itself, but when you think about it, using the FVAS id makes sense : A1 and A2 are transmited by the D4R-II (wheter you want them or not), you cannot use FLVSS because it measure individual cells, not the total, that leaves you VFAS free for a full voltage transmition.

- gps coords are fine
- speed : I'm working on it to undo the conversion to knots and trying to send km/h (or possibly m/s), but I first need to find out what unit MW is operating with
- course works now very well : pre 2.1, there was an issue with negative heading value, with 2.1 they are properly handled by Taranis without changing your code
- amp : my board cannot measure that
- AccX,Y,Z : perfect
- Temperature : perfect
- Time : working, transmiting the cummulative total armed time since lipo connected. The resullt gets into a field that combines date and time, and the date part stays at 00-0-0 (not an issue)
- Distance to home sent as RPM : working very well, you just have to rename the sensor on the Taranis side to Dist and set the unit to meter.
- Numsat as temp2 : working very well, you just have to rename the sensor on the Taranis side to Sats and set the unit to nothing.

Thanks a LOT for your help,

G

QuadBow
Posts: 532
Joined: Fri Jan 04, 2013 10:06 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by QuadBow »

Hi gandalf33,

Good to hear that almost everthing is working fine for your configuration, too.

Concerning the speed I am sending the knots due to http://www.frsky-rc.com/download/down.php?id=126, which is calculated from centimeter per second with rounding. My display (Frsky FLD-02) can convert from knots to kilometer per hour.

So, let me know, if OpenTX follows another approach and expects something else than knots.

gandalf33
Posts: 10
Joined: Thu Jul 02, 2015 9:12 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by gandalf33 »

Well, starting with 2.1, OpenTX expect, well, nothing really, you can choose what unit you are receiving, they just need to match between the sender and the Taranis. I do not think the Taranis can convert knots to kmh easely (yes, I can write lua to do that, but I would not qualify that as easy) The easiest to 'understand' speed for 'metric' people like me is probably km/h. Maybe you can introduce a choice in the config.h TELEMETRY_GSPEED_KMH, if defined then that's what you convert too and send, else your current knots sending, would that work ?

G

szakacs
Posts: 18
Joined: Wed Dec 11, 2013 11:53 pm
Location: Sydney, NSW Australia

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by szakacs »

LutzB wrote:
szakacs wrote:I am trying to implement this on a HK Multiwii Pro board and a X8R receiver.
I believe I have configured and uploaded the Multiwii 2.4 with S.Port correctly.
I have built an inverter as descibed here http://i.imgur.com/n7iS5LW.png.
My problem is that as soon as I connect the inverter the MWC freezes/crashes.
I have found that as soon as I connect the S.Port signal to the inverter the signal leds, led2 and led3 on the MWC board flash quickly for a second and then stay full on.
As a test I seperated the tx / rx signal from the S.Port. As soon as I connect the Rx side to the S.Port the MWC board freezes.
Am I missing a step somewhere ?? any help would be appreciated.

This looks to me more like a hardware problem. Can you check that there is no short in your circuit? I built my converter with the same diagram, so I can confirm that it is working.

I will test the code in the trunk this weekend just to be shure.


Thank you for your replies...
I think I have found my issue, but as usual I cannot explain why.
I like to play on the test bench and have a habit of playing around with all available options.
What I had done was activated LCD telementry a while ago, and playing / proving that it was working....All good.
The LCD telementry activated I2C communications and that somehow affected the Rx signal on all serial ports when connecting the S.port inverter.
Turning off the LCD telementry stopped the freezing / crashing issue.
Now to see what data is useful ...

QuadBow
Posts: 532
Joined: Fri Jan 04, 2013 10:06 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by QuadBow »

Hi all,

I have received an hint of one user that OpenTX prefers a speed format in kilometers per hour instead of knots.
Therefore, I have added the definition KILOMETERS_HOUR in the telemetry section of the file config.h.
The default selection will still be knots. Kilometers per hour are not selected automatically when using OpenTX.
The changes have been uploaded into the main trunk under r1778.

Please, test it and let me know, if you have proposals for improvement.

gandalf33
Posts: 10
Joined: Thu Jul 02, 2015 9:12 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by gandalf33 »

I've done a quick test, and so far, it looks great on the Taranis, but I won't be flying before wednesday, so we will know more then. Then next round of test will be when I receive my pre-ordered X9E :D

G

QuadBow
Posts: 532
Joined: Fri Jan 04, 2013 10:06 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by QuadBow »

Hi gandalf33,
It's good to hear that it looks great for you. ;)
I am still awaiting test results and wishes of other users with other configurations.

gandalf33
Posts: 10
Joined: Thu Jul 02, 2015 9:12 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by gandalf33 »

Hello quadbow,

Looking at opentx github this morning, I noticed this : https://github.com/opentx/opentx/commit ... e444506879

By the look of it, there was an issue preventing voltage to be properly read using 0x3A and 0xB, but that should be fixed in 2.1.1, I'll let you know when this is released and I have opportunity to test

G

LutzB
Posts: 67
Joined: Sun Feb 08, 2015 4:01 pm
Location: Germany

Code Refactoring?

Post by LutzB »

While looking for the S.Port telemetry code, I found the handy libraries for S.Port and old FrSky telemetry on rcgroups. I asked myself, why we must invent the wheel again?

Is there any chance that something like this will be merged into the multiwii code? I'd really like to do it, but before spending too much time on this, I would like to know, if it would be accepted. The coding style is very different and lots of small files are used, but this makes the lib readable and easy to use. Any comments are welcome.

QuadBow
Posts: 532
Joined: Fri Jan 04, 2013 10:06 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by QuadBow »

Hi LutzB,

Thank you very much for your findings. I also agree with you not to reinvent the wheel.
After I have had a look inside the mentioned libraries, I think it is a lot of effort to adapt the code to the multiwii structure.

There are some severe differences between the library and Multiwii code.
    The library is written in an object oriented way - Multiwii not.
    The library is split up into many files.
    And it is rather specific to Naza/Teensy/etc.

In my view, we had to change a lot of Multiwii code without gaining much.
So, my proposal would be to look into the content for new functions which have not been implemented in Multiwii instead of reusing the code.

LutzB
Posts: 67
Joined: Sun Feb 08, 2015 4:01 pm
Location: Germany

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by LutzB »

Thanks to this rainy sunday I reworked the SPort telemetry according to the mentioned library. Only the delays for each value are not done, but it is working fine here with my Taranis Plus.

As I changed the code to simulate the FrSky hardware sensors, most IDs have changed. If you setup your radio already, you will have to do it again. The default IDs of the hardware sensors are now used. You can change this in config.h with the "FRSKY_SPORT_OVERRIDE_DEVICE_XXX" definitions. The definition for the A2 scale is not needed anymore, as I now transmit the data with the ID of FCS sensor. So this is really "Plug'n'Play": activate SPORT_TELEMETRY, set your serial port and you are done. Everything MultiWii measures according to your definitions in config.h will be transmitted - if supported.

Some values are just a guess, as I had no idea of what unit MultiWii uses to store them:
  • Vario sensor
  • Current of FCS sensor
  • GPS speed
If there is someone using these, please give a reply if the value is correct. I have no hardware here for this. I am also interested in the result on different radios. Please give some feedback.

Telemetry part in config.h:

Code: Select all

  /********************************************************************/
  /****                             TELEMETRY                      ****/
  /********************************************************************/
    // select one of the two protocols depending on your receiver
    //#define FRSKY_TELEMETRY         // used for FRSKY twoway receivers with telemetry (D-series like D8R-II or D8R-XP)
                                      // VBAT, Baro, MAG, GPS and POWERMETER are helpful
                                      // VBAT_CELLS is optional for a forth screen on the display FLD-02
    #define SPORT_TELEMETRY         // for FRSKY twoway receivers with S.PORT telemetry (X-series like X4R/X6R/X8R)
                                      // definitions        simulated sensor
                                      // Baro, VARIO     -> Vario
                                      // VBAT,POWERMETER -> FCS
                                      // VBAT_CELLS      -> FLVSS
                                      // GPS             -> GPS
                                      // ACC             -> ACC
                                      // MAG             -> heading via second GPS

    // FRSKY common entries - valid for both protocols
    #define TELEMETRY_SERIAL 3        // change if required

    // FRSKY standard telemetry specific devices
    //#define FRSKY_FLD02               // send only data specific for the FRSKY display FLD-02
    //#define OPENTX                    // send OpenTX specific data

    // FRSKY standard telemetry specific selections
    //#define COORDFORMAT_DECIMALMINUTES // uncomment to get the format DD°MM.mmmm for the coordinates - comment out to get the format DD.dddddd° for the coordinates
    //#define KILOMETER_HOUR            // send speed in kilometers per hour instead of knots (default) - requested by OPENTX
    //#define TELEMETRY_ALT_BARO        // send BARO based altitude, calibrated to 0 when arming, recommended if BARO available
    //#define TELEMETRY_ALT_GPS         // send GPS based altitude (altitude above see level), for FLD-02 don't use together with TELEMETRY_ALT_BARO
    //#define TELEMETRY_COURSE_MAG      // send MAG based course/heading, recommended if MAG available, but FLD-02 does not display
    //#define TELEMETRY_COURSE_GPS      // send GPS based course/heading, don't use together with TELEMETRY_COURSE_MAG, FLD-02 does not display

    // S.PORT specific entries
    // Override default simulated sensor ID. Usefull if you have hardware sensors with same ID installed.
    //#define FRSKY_SPORT_OVERRIDE_DEVICE_VARIO     FRSKY_SPORT_DEVICE_1    // default 1
    //#define FRSKY_SPORT_OVERRIDE_DEVICE_FLVSS     FRSKY_SPORT_DEVICE_2    // default 2
    //#define FRSKY_SPORT_OVERRIDE_DEVICE_FCS       FRSKY_SPORT_DEVICE_3    // default 3
    //#define FRSKY_SPORT_OVERRIDE_DEVICE_GPS       FRSKY_SPORT_DEVICE_4    // default 4
    //#define FRSKY_SPORT_OVERRIDE_DEVICE_RPM       FRSKY_SPORT_DEVICE_5    // default 5 TODO
    //#define FRSKY_SPORT_OVERRIDE_DEVICE_SP2UART   FRSKY_SPORT_DEVICE_7    // default 7 TODO
    //#define FRSKY_SPORT_OVERRIDE_DEVICE_ACC       FRSKY_SPORT_DEVICE_20   // default 20
    //#define FRSKY_SPORT_OVERRIDE_DEVICE_MAG       FRSKY_SPORT_DEVICE_21   // default 21


telemetry.h:

Code: Select all

#if defined(SPORT_TELEMETRY)

    #define TELEMETRY_BAUD   57600 
    #define FRSKY_START_STOP  0x7e
    #define FRSKY_BYTESTUFF   0x7d
    #define FRSKY_STUFF_MASK  0x20

    // FrSky data IDs (2 bytes)
    #define FRSKY_SPORT_RSSI_ID           0xf101 // used by rx
    #define FRSKY_SPORT_ADC1_ID           0xf102 // used by rx
    #define FRSKY_SPORT_ADC2_ID           0xf103 // used by rx
    #define FRSKY_SPORT_BATT_ID           0xf104 // used by rx
    #define FRSKY_SPORT_SWR_ID            0xf105 // used by tx
    #define FRSKY_SPORT_T1_ID             0x0400
    #define FRSKY_SPORT_T2_ID             0x0410
    #define FRSKY_SPORT_RPM_ID            0x0500
    #define FRSKY_SPORT_FUEL_ID           0x0600
    #define FRSKY_SPORT_ALT_ID            0x0100 // used by Vario
    #define FRSKY_SPORT_VARIO_ID          0x0110 // used by vario
    #define FRSKY_SPORT_ACCX_ID           0x0700
    #define FRSKY_SPORT_ACCY_ID           0x0710
    #define FRSKY_SPORT_ACCZ_ID           0x0720
    #define FRSKY_SPORT_CURR_ID           0x0200 // used by FCS
    #define FRSKY_SPORT_VFAS_ID           0x0210 // used by FCS
    #define FRSKY_SPORT_CELLS_ID          0x0300 // used by FLVSS
    #define FRSKY_SPORT_GPS_LONG_LATI_ID  0x0800 // used by GPS
    #define FRSKY_SPORT_GPS_ALT_ID        0x0820 // used by GPS
    #define FRSKY_SPORT_GPS_SPEED_ID      0x0830 // used by GPS
    #define FRSKY_SPORT_GPS_COURS_ID      0x0840 // used by GPS
    #define FRSKY_SPORT_GPS_TIME_DATE_ID  0x0850 // used by GPS

    // FrSky sensor IDs (this also happens to be the order in which they're broadcast from an X8R)
    // NOTE: As FrSky puts out more sensors let's try to add comments here indicating which is which
    #define FRSKY_SPORT_DEVICE_1   0x00 // Variometer
    #define FRSKY_SPORT_DEVICE_2   0xa1 // FLVSS
    #define FRSKY_SPORT_DEVICE_3   0x22 // FCS
    #define FRSKY_SPORT_DEVICE_4   0x83 // GSP
    #define FRSKY_SPORT_DEVICE_5   0xe4 // RPM
    #define FRSKY_SPORT_DEVICE_6   0x45
    #define FRSKY_SPORT_DEVICE_7   0xc6 // SP2UART
    #define FRSKY_SPORT_DEVICE_8   0x67
    #define FRSKY_SPORT_DEVICE_9   0x48
    #define FRSKY_SPORT_DEVICE_10  0xe9
    #define FRSKY_SPORT_DEVICE_11  0x6a
    #define FRSKY_SPORT_DEVICE_12  0xcb
    #define FRSKY_SPORT_DEVICE_13  0xac
    #define FRSKY_SPORT_DEVICE_14  0xd
    #define FRSKY_SPORT_DEVICE_15  0x8e
    #define FRSKY_SPORT_DEVICE_16  0x2f
    #define FRSKY_SPORT_DEVICE_17  0xd0
    #define FRSKY_SPORT_DEVICE_18  0x71
    #define FRSKY_SPORT_DEVICE_19  0xf2
    #define FRSKY_SPORT_DEVICE_20  0x53
    #define FRSKY_SPORT_DEVICE_21  0x34
    #define FRSKY_SPORT_DEVICE_22  0x95
    #define FRSKY_SPORT_DEVICE_23  0x16
    #define FRSKY_SPORT_DEVICE_24  0xb7
    #define FRSKY_SPORT_DEVICE_25  0x98 // rx
    #define FRSKY_SPORT_DEVICE_26  0x39
    #define FRSKY_SPORT_DEVICE_27  0xba // A2 on rx X4R
    #define FRSKY_SPORT_DEVICE_28  0x1b

    // Default IDs for simulated sensors
    #define FRSKY_SPORT_DEVICE_VARIO    FRSKY_SPORT_DEVICE_1
    #define FRSKY_SPORT_DEVICE_FLVSS    FRSKY_SPORT_DEVICE_2
    #define FRSKY_SPORT_DEVICE_FCS      FRSKY_SPORT_DEVICE_3
    #define FRSKY_SPORT_DEVICE_GPS      FRSKY_SPORT_DEVICE_4
    #define FRSKY_SPORT_DEVICE_RPM      FRSKY_SPORT_DEVICE_5
    #define FRSKY_SPORT_DEVICE_SP2UART  FRSKY_SPORT_DEVICE_7
    #define FRSKY_SPORT_DEVICE_ACC      FRSKY_SPORT_DEVICE_20
    #define FRSKY_SPORT_DEVICE_MAG      FRSKY_SPORT_DEVICE_21

    // Override default IDs
    // Vario
    #ifdef  FRSKY_SPORT_OVERRIDE_DEVICE_VARIO
      #undef  FRSKY_SPORT_DEVICE_VARIO
      #define FRSKY_SPORT_DEVICE_VARIO FRSKY_SPORT_OVERRIDE_DEVICE_VARIO
    #endif
    // FLVSS
    #ifdef  FRSKY_SPORT_OVERRIDE_DEVICE_FLVSS
      #undef  FRSKY_SPORT_DEVICE_FLVSS
      #define FRSKY_SPORT_DEVICE_FLVSS FRSKY_SPORT_OVERRIDE_DEVICE_FLVSS
    #endif
    // FCS
    #ifdef  FRSKY_SPORT_OVERRIDE_DEVICE_FCS
      #undef  FRSKY_SPORT_DEVICE_FCS
      #define FRSKY_SPORT_DEVICE_FCS FRSKY_SPORT_OVERRIDE_DEVICE_FCS
    #endif
    // GPS
    #ifdef  FRSKY_SPORT_OVERRIDE_DEVICE_GPS
      #undef  FRSKY_SPORT_DEVICE_GPS
      #define FRSKY_SPORT_DEVICE_GPS FRSKY_SPORT_OVERRIDE_DEVICE_GPS
    #endif
    // RPM
    #ifdef  FRSKY_SPORT_OVERRIDE_DEVICE_RPM
      #undef  FRSKY_SPORT_DEVICE_RPM
      #define FRSKY_SPORT_DEVICE_RPM FRSKY_SPORT_OVERRIDE_DEVICE_RPM
    #endif
    // SP2UART
    #ifdef  FRSKY_SPORT_OVERRIDE_DEVICE_SP2UART
      #undef  FRSKY_SPORT_DEVICE_SP2UART
      #define FRSKY_SPORT_DEVICE_SP2UART FRSKY_SPORT_OVERRIDE_DEVICE_SP2UART
    #endif
    // ACC
    #ifdef  FRSKY_SPORT_OVERRIDE_DEVICE_ACC
      #undef  FRSKY_SPORT_DEVICE_ACC
      #define FRSKY_SPORT_DEVICE_ACC FRSKY_SPORT_OVERRIDE_DEVICE_ACC
    #endif
    // MAG
    #ifdef  FRSKY_SPORT_OVERRIDE_DEVICE_MAG
      #undef  FRSKY_SPORT_DEVICE_MAG
      #define FRSKY_SPORT_DEVICE_MAG FRSKY_SPORT_OVERRIDE_DEVICE_MAG
    #endif
#endif // S.PORT telemetry


telemetry.cpp:

Code: Select all

#if defined(SPORT_TELEMETRY)

  static short _FrSkySport_crc;
  #ifdef ACC
    static short _currentACCValue;
  #endif
  #ifdef BARO
    static short _currentVarioValue;
  #endif
  #if GPS
    static short _currentGPSValue;
  #endif
  #ifdef VBAT_CELLS
    static short _currentFLVSSValue;
  #endif
  #ifdef VBAT || POWERMETER
    static short _currentFCSValue;
  #endif

  void FrSkySport_sendByte(uint8_t byte)
  {
    // CRC update
    _FrSkySport_crc += byte; //0-1FF
    _FrSkySport_crc += _FrSkySport_crc >> 8; //0-100
    _FrSkySport_crc &= 0x00ff;
    _FrSkySport_crc += _FrSkySport_crc >> 8; //0-0FF
    _FrSkySport_crc &= 0x00ff;

    if ( (byte == FRSKY_START_STOP) || (byte == FRSKY_BYTESTUFF) ) {
      SerialWrite(TELEMETRY_SERIAL, FRSKY_BYTESTUFF);
      byte &= ~FRSKY_STUFF_MASK;
    }

    SerialWrite(TELEMETRY_SERIAL, byte);
  }

  void FrSkySport_sendCrc()
  {
    FrSkySport_sendByte(0xFF - _FrSkySport_crc);
  }

  void FrSkySport_sendValue(uint16_t id, uint32_t value)
  {
    _FrSkySport_crc = 0; // Reset CRC
    FrSkySport_sendByte(0x10); // DATA_FRAME
    uint8_t *bytes = (uint8_t*)&id;
    FrSkySport_sendByte(bytes[0]);
    FrSkySport_sendByte(bytes[1]);
    bytes = (uint8_t*)&value;
    FrSkySport_sendByte(bytes[0]);
    FrSkySport_sendByte(bytes[1]);
    FrSkySport_sendByte(bytes[2]);
    FrSkySport_sendByte(bytes[3]);
    FrSkySport_sendCrc();
  }

  void FrSkySport_sendEmpty(uint16_t id)
  {
    FrSkySport_sendByte(0x00);
    uint8_t *bytes = (uint8_t*)&id;
    FrSkySport_sendByte(bytes[0]);
    FrSkySport_sendByte(bytes[1]);
    for(uint8_t i = 0; i < 4; i++)
      FrSkySport_sendByte(0x00);
    FrSkySport_sendCrc();
  }

  void FrSkySport_Vario()
  {
  #if BARO
    switch(_currentVarioValue){
      case 0:
        FrSkySport_sendValue(FRSKY_SPORT_ALT_ID, (int32_t)(alt.EstAlt) * 100); // cm
        break;
      case 1:
      #ifdef VARIOMETER
        FrSkySport_sendValue(FRSKY_SPORT_VARIO_ID, ((uint32_t)alt.vario)); // unknown unit
      #else
        FrSkySport_sendEmpty(FRSKY_SPORT_VARIO_ID);
      #endif
        break;
    }
    #ifdef VARIOMETER
      _currentVarioValue = ++_currentVarioValue % 2;
    #endif
  #endif
  }

  void FrSkySport_FLVSS()
  {
  /*
   * requirement:
   *      analog.vbatcells[0] = cell1
   *      analog.vbatcells[1] = cell1 + cell2
   *      analog.vbatcells[2] = cell1 + cell2 + cell3
   *      analog.vbatcells[3] = cell1 + cell2 + cell3 +cell4
   *      analog.vbatcells[4] = cell1 + cell2 + cell3 +cell4 + cell5
   *      analog.vbatcells[5] = cell1 + cell2 + cell3 +cell4 + cell5 + cell6
   */
  #ifdef VBAT_CELLS
    uint8_t firstCellNo = _currentFLVSSValue * 2;
    uint16_t cell1Data = 0;
    uint16_t cell2Data = 0;
    uint32_t cellData = 0;
    if(firstCellNo == 0){
        cell1Data = analog.vbatcells[firstCellNo] * 50;
    } else {
      cell1Data = (analog.vbatcells[firstCellNo] - analog.vbatcells[firstCellNo-1]) * 50;
    }
    if(VBAT_CELLS_NUM > (firstCellNo+1))
      cell2Data = (analog.vbatcells[firstCellNo+1] - analog.vbatcells[firstCellNo]) * 50;

    cellData = cell2Data & 0x0FFF;
    cellData <<= 12;
    cellData |= cell1Data & 0x0FFF;
    cellData <<= 4;
    cellData |= VBAT_CELLS_NUM & 0x0F;
    cellData <<= 4;
    cellData |= firstCellNo & 0x0F;

    if(VBAT_CELLS_NUM > (firstCellNo+2)){
      _currentFLVSSValue++;
    } else {
      _currentFLVSSValue = 0;
    }
    FrSkySport_sendValue(FRSKY_SPORT_CELLS_ID, cellData);
  #endif
  }

  void FrSkySport_FCS()
  {
  #ifdef VBAT || POWERMETER
    switch(_currentFCSValue){
      case 0:
      #ifdef VBAT
        FrSkySport_sendValue(FRSKY_SPORT_VFAS_ID, (uint32_t)(analog.vbat * 10));
      #else
        FrSkySport_sendEmpty(FRSKY_SPORT_VFAS_ID);
      #endif
        break;
      case 1:
      #ifdef POWERMETER
        FrSkySport_sendValue(FRSKY_SPORT_CURR_ID, (uint32_t)analog.amperage); // not tested! must be A*10
      #else
        FrSkySport_sendEmpty(FRSKY_SPORT_CURR_ID);
      #endif
        break;
    }
    _currentFCSValue = ++_currentFCSValue % 2;
  #endif
  }

  uint32_t FrSkySport_EncodeCoordinate(float latLon, bool isLat)
  {
  #if GPS
    uint32_t coord = 0;
    if (!isLat)
    {
      coord = abs(latLon);  // now we have unsigned value and one bit to spare
      coord = (coord + coord / 2) / 25 | 0x80000000;  // 6/100 = 1.5/25, division by power of 2 is fast
      if (latLon < 0) coord |= 0x40000000;
    }
    else {
      coord = abs(latLon);  // now we have unsigned value and one bit to spare
      coord = (coord + coord / 2) / 25;  // 6/100 = 1.5/25, division by power of 2 is fast
      if (latLon < 0) coord |= 0x40000000;
    }
    return coord;
  #endif
  }

  void FrSkySport_GPS()
  {
  #if GPS
    uint32_t GPSValueToSend = 0;
    uint16_t GPSId = 0;

    switch(_currentGPSValue){
      case 0: // latitude
        GPSValueToSend = FrSkySport_EncodeCoordinate(GPS_coord[LON], false);
        GPSId = FRSKY_SPORT_GPS_LONG_LATI_ID;
        break;
      case 1: // longitude
        GPSValueToSend = FrSkySport_EncodeCoordinate(GPS_coord[LAT], true);
        GPSId = FRSKY_SPORT_GPS_LONG_LATI_ID;
        break;
      case 2: // altitude
        GPSValueToSend = (int32_t)(GPS_altitude * 100); // meter
        GPSId = FRSKY_SPORT_GPS_ALT_ID;
        break;
      case 3: // speed
        GPSValueToSend = ((float)GPS_speed * 1000); // TODO: unknown unit, gives same numbers as MultiWiiConf
        GPSId = FRSKY_SPORT_GPS_SPEED_ID;
        break;
      case 4: // course over ground
        GPSValueToSend = (uint32_t)(GPS_ground_course + 360) % 360 * 100; // 1 deg = 100, 0 - 359000
        GPSId = FRSKY_SPORT_GPS_COURS_ID;
        break;
    }
    if (f.GPS_FIX && GPS_numSat >= 4) {
      FrSkySport_sendValue(GPSId, GPSValueToSend);
    } else {
      FrSkySport_sendEmpty(GPSId);
    }
    _currentGPSValue = ++_currentGPSValue % 5;
  #endif
  }

/*
  void FrSkySport_RPM()
  {
    // TODO
  }

  void FrSkySport_SP2UART()
  {
    // TODO
  }
*/

  void FrSkySport_ACC()
  {
  #ifdef ACC
    switch(_currentACCValue){
      case 0:
        FrSkySport_sendValue(FRSKY_SPORT_ACCX_ID, imu.accSmooth[0] / (ACC_1G / 100));
        break;
      case 1:
        FrSkySport_sendValue(FRSKY_SPORT_ACCY_ID, imu.accSmooth[1] / (ACC_1G / 100));
        break;
      case 2:
        FrSkySport_sendValue(FRSKY_SPORT_ACCZ_ID, imu.accSmooth[2] / (ACC_1G / 100));
        break;
    }
    _currentACCValue = ++_currentACCValue % 3;
  #endif
  }

  void FrSkySport_MAGHeading()
  {
  #ifdef MAG
    FrSkySport_sendValue(FRSKY_SPORT_GPS_COURS_ID, (uint32_t)(att.heading + 360) % 360 * 100); // 1 deg = 100, 0 - 359000
  #endif
  }

  void init_telemetry(void)
  {
  #ifdef ACC
    _currentACCValue = 0;
  #endif
  #ifdef BARO
    _currentVarioValue = 0;
  #endif
  #if GPS
    _currentGPSValue = 0;
  #endif
  #ifdef VBAT_CELLS
    _currentFLVSSValue = 0;
  #endif
  #ifdef VBAT || POWERMETER
    _currentFCSValue = 0;
  #endif

    SerialOpen(TELEMETRY_SERIAL,TELEMETRY_BAUD);
  }

  void run_telemetry(void)
  {
    static uint8_t lastRx = 0;
    uint8_t c = SerialAvailable(TELEMETRY_SERIAL);

    while (c--) {
      int rx = SerialRead(TELEMETRY_SERIAL);
      if (lastRx == FRSKY_START_STOP)
      {
        switch (rx)
        {
          case FRSKY_SPORT_DEVICE_VARIO: // Variometer
              FrSkySport_Vario();
            break;
          case FRSKY_SPORT_DEVICE_FLVSS: // FLVSS
              FrSkySport_FLVSS();
            break;
          case FRSKY_SPORT_DEVICE_FCS: // FCS-40A/FCS-150A
              FrSkySport_FCS();
            break;
          case FRSKY_SPORT_DEVICE_GPS: // GPS
              FrSkySport_GPS();
            break;
          //case FRSKY_SPORT_DEVICE_RPM: // RPM TODO
          //  FrSkySport_RPM();        // provides RPM + 2 temperatures
          //  break;
          //case FRSKY_SPORT_DEVICE_SP2UART: // S.Port tu UART TODO
          //  FrSkySport_SP2UART();    // provides 2 temperatures
          //  break;
          case FRSKY_SPORT_DEVICE_ACC: // ACC
              FrSkySport_ACC();
            break;
          case FRSKY_SPORT_DEVICE_MAG: // MAG heading implemented as second GPS device
              FrSkySport_MAGHeading();
            break;
        }
      }
      lastRx = rx;
    }
  }
#endif // S.PORT telemetry


Have FUN!

Lutz

QuadBow
Posts: 532
Joined: Fri Jan 04, 2013 10:06 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by QuadBow »

Hi LutzB,
I took note on your effort on S.PORT telemetry improvement and will commit it into the main trunk after some positive feedbacks.
(I don't have the new S.PORT equipment, so I can not contribute to testing.)
Have fun (despite the rain)

LutzB
Posts: 67
Joined: Sun Feb 08, 2015 4:01 pm
Location: Germany

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by LutzB »

Thanks QuadBow!

Maybe I should tell you that I still moved the run_telemetry() call to the position mentioned earlier. The result is much better then.
I think timing is much more important for SPort because the receiver polls the sensors. I have no idea in what timeframe the answer has to be send. You can see the result perfect with opentx 2.1.1. I got sometimes duplicates of the sensors with different ids. This can only happen, when another id is polled before the response of the last was send. After moving the function call the duplicates were gone.

Lutz

QuadBow
Posts: 532
Joined: Fri Jan 04, 2013 10:06 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by QuadBow »

Hi Lutz,
I remember that you mentioned it already. In principle I do not have any issue with that.
But, it should be well tested in order to avoid odd behaviour of other functions.

Howie
Posts: 2
Joined: Thu Jul 19, 2012 3:19 pm
Location: Düsseldorf, Germany
Contact:

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by Howie »

LutzB wrote:Thanks to this rainy sunday I reworked the SPort telemetry according to the mentioned library. Only the delays for each value are not done, but it is working fine here with my Taranis Plus.

Have FUN!

Lutz

Hello Lutz,

i am trying to get this to work, but with no success so far.
on what kind of boards / processors did you run your tests ?

Telemetry part in config.h:

Code: Select all

    #define TELEMETRY_SERIAL 3        // change if required


not quite clear, this is the number of the serial port, not the pin to connect to ?
so this was runnng on a 2560 with 4 serial ports, right ?

Thanks

Howie

LutzB
Posts: 67
Joined: Sun Feb 08, 2015 4:01 pm
Location: Germany

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by LutzB »

You are absolutely right. I have a Crius AIO Pro v2.0 with the Mega processor.
So you need a free serial port for this.

Howie
Posts: 2
Joined: Thu Jul 19, 2012 3:19 pm
Location: Düsseldorf, Germany
Contact:

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by Howie »

ok, i was testing on a 32u4 board, that has a second UART

and because of the hardware UART i have to use an inverter...
I think i'm getting closer :-)

I was testing some other stuff with the openXsensor before, that kind of confused me because of the SoftwareSerial without the need for an inverter...

QuadBow
Posts: 532
Joined: Fri Jan 04, 2013 10:06 am

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by QuadBow »

Howie wrote:I was testing some other stuff with the openXsensor before, that kind of confused me because of the SoftwareSerial without the need for an inverter...
It has alerady been found out in http://www.multiwii.com/forum/viewtopic.php?f=7&t=1929&p=62027&hilit=direct+frsky+software+serial#p60925, that software serial results in undesired behaviour...

Maine_Guy
Posts: 27
Joined: Fri Mar 06, 2015 7:07 pm

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by Maine_Guy »

So,

I've updated the code to support the opentx output. I've been hi-jacking telemetry output to give me feedback on various operational parameters - like PID settings.. etc.

I've tried to output data on the GPS altitude telemetry variable - with no luck. I'm trying to output the althold variable (altitude attempting to hold, which should be the alitutde variable at time of activation) but I get nothing but zero (0) on the telemetry output.

Any ideas? Anything special about the GPS altitude telemetry value?

multicopter1
Posts: 3
Joined: Thu Mar 10, 2016 8:25 pm

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by multicopter1 »

Hi all,

can anyone please give me the latest code for SPORT telemetry?
The whole telemetry libraries, which i have to put into MultiWii v2.4 would be the best, because a lot of codes posted seem to be removed.

And which inverter do i have to use in order to connect my mega board to the X8R?
Can i use this http://www.ebay.com/itm/181878009699?_t ... EBIDX%3AIT or do I have to use the inverter described on this topic viewtopic.php?f=8&t=4507 ? Or am I even able to use an SBus cable?

I´m looking forward to your answers! ;)

Jonas
Last edited by multicopter1 on Sat Mar 12, 2016 10:52 am, edited 2 times in total.

LutzB
Posts: 67
Joined: Sun Feb 08, 2015 4:01 pm
Location: Germany

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by LutzB »

Please find the latest MultiWii 2.4 with S.Port Telemetry in my Git repo in the S.Port-Telemetry branch:
https://github.com/benderl/multiwii-fir ... -Telemetry

You will need an Inverter as described in the topic you referenced to. The TTL level converter will not invert the signal and cannot be used. The SBus cable is only for one way communication and S.Port need TX and RX lines.

multicopter1
Posts: 3
Joined: Thu Mar 10, 2016 8:25 pm

Re: FRSKY telemetry integration into Multiwii 2.4 has starte

Post by multicopter1 »

Hi LutzB,

thank you very much!
You have answered all my questions! :D

Jonas

Post Reply