Direct Frsky telemtry data from MW FC

ezio · Post by **ezio** » Tue Oct 09, 2012 11:38 pm

Hi
it is save if I use it with ATMega324 and I have changed serial speed to 9600 ?
it doesn't make longer main loop intervals ?

ezio · Post by **ezio** » Sat Oct 13, 2012 2:55 am

I have modified frsky telemetry a little bit to be compatible with my android app.
file is here https://github.com/eziosoft/MultiWii_EZ ... lemtry.ino

janekx · Post by **janekx** » Sat Oct 13, 2012 11:05 am

This change brake the Acc X/Y/Z telemetry data sended to Frsky modules.

kataventos · Post by **kataventos** » Mon Oct 22, 2012 1:26 pm

wilco1967 wrote:

Thanks for this great idea/code

I have made this inverter for other project and seems to me that this work either, but I wanted to ask for a second opinion, sorry for the hi tec hand draw

:

: Inverter

Thanks

vpb · Post by **vpb** » Wed Oct 31, 2012 2:48 pm

Here is my telemetry test with r1177, latest frskytelemetry.ino from janekx, level converter from wilco1967 it works very well, thank you all!!!
https://www.youtube.com/watch?v=MeIb-4KfoqI

Commander1024 · Post by **Commander1024** » Thu Nov 01, 2012 5:09 pm

Hi, I modified the scematic above to do both: level converting and inverting

UPDATED: fixed misplaced resistors

kataventos · Post by **kataventos** » Thu Nov 01, 2012 8:07 pm

Commander1024 wrote:Hi, I modified the scematic above to do both: level converting and inverting

thanks and welcome, you are right!

djrm · Post by **djrm** » Thu Nov 01, 2012 8:10 pm

Here is a simple way to use both a Bluetooth transmitter and Er9x transmitter LCD together using an RS232 Bluetooth adapter, the adapter has pins for the ttl level and the inverted RS232 level serial signals, so combining both a level translator for the RC and the Bluetooth transmitter, they both seem to work together without problems.

The HC-05-D RS232 bluetooth adapter is listed to buy online, mine came from China for $19.90 including some cables and carriage. The default settings did not need changing, it is supplied working at 9600 bps.

The Bluetooth module connects to both the USBasp programming port and the Frsky transmitter modules using these pin assignments:
1 +5v from usbasp pin 2
2 no connection
3 Rx ttl to usbasp pin 1
4 Tx ttl to usbasp pin 9
5 no connection
6 RX RS232 to Frsky Txd
7 TX RS232 to Frsky Rxd
8 Gnd to usbasp pin 10

The connections to the usbasp programming port are the ttl serial data connections needed by the Er9x - Frsky modified RC transmitter to show the telemetry data on the RC screen. with this setup I could view the telemetry on the Er9x screen as well as with my mobile Bluetooth application for logging.

Its early days for this at present, I'm waiting for a chance to fly my new Naze32 quad with this gear.
hth David

kataventos · Post by **kataventos** » Thu Nov 01, 2012 11:53 pm

OK, downlink done... it´s just tweaking and work together with the ER9x team for more "options"!

What about Alex idea of using Frsky on an uplink without disturb the 8 ch we have?
This is something usefull for way point nav, and our modules have good range for all of that! It´s this possible? any ideas?

IceWind · Post by **IceWind** » Fri Nov 02, 2012 12:13 am

For simple operations yes, for waypoints could be tricky as it would easier to have a map to select/edit points rather than the radio.

Btw I made a different connection involving less cables.

Straight into a RS232<>TTL converter.

Still need to reduce the cables size a bit.

copterrichie · Post by **copterrichie** » Fri Nov 02, 2012 1:49 am

Hey guys, this is a little off topic but it is about Bluetooth hacking. I have done it but have not fully validated the range. I read, it works best if both ends are modified.

http://www.youtube.com/watch?v=mddqIHH24TU

http://www.instructables.com/id/Increas ... uetooth-d/

postaL · Post by **postaL** » Fri Nov 02, 2012 1:59 am

copterrichie wrote:Hey guys, this is a little off topic but...

You should start all your posts with this.

ps. the CLAN is WATCHING.

copterrichie · Post by **copterrichie** » Fri Nov 02, 2012 2:07 am

postaL wrote:
ps. the CLAN is WATCHING.

Good, maybe you might learn something. As if I cared. Ha!

kataventos · Post by **kataventos** » Fri Nov 02, 2012 3:59 am

copterrichie wrote:Hey guys, this is a little off topic but it is about Bluetooth hacking. I have done it but have not fully validated the range. I read, it works best if both ends are modified.

http://www.youtube.com/watch?v=mddqIHH24TU

If we could avoid more parts... and you will never have the power you need on FPV, would be awesome to use just the Frsky TX<->RX

:mrgreen:

I like the end of the video, I will try that gain antenna with my cat :lol:

kataventos · Post by **kataventos** » Thu Nov 15, 2012 3:03 am

Sebbi wrote:It's not 2.1 compatible ... or at least not compatible with the current _shared branch. I haven't tested it yet, but my changes so the code compiles again were:

FrSkyTelemtry.ino (write_FrSky8 just uses SerialWrite() now):

Code: Select all

// ****************************************************************
// FrSky telemetry
// Version: 0.2.1
// Date 20/09/2012
// Changes: V0.2.1: - make it work with 2.1 (shared dev)
// Date: 14/08/2012
// Changes: V0.2: - Byte stuffing added
//                - vBat will be send, if "#define FAS_100" is comment out
//                V0.1: - First release
// ****************************************************************

#if defined(TELEMETRY_FRSKY)
  // user defines
  //#define FAS_100  //if commment out, MultiWii vBat voltage will be send instead of FrSky FAS 100 voltage


  // Serial config datas
  #define TELEMETRY_FRSKY_SERIAL 1
  #define TELEMETRY_FRSKY_BAUD 9600  

  // Timing
  #define Time_telemetry_send 125000 
  static uint8_t cycleCounter = 0;
  static uint32_t FrSkyTime  = 0;

  // Frame protocol
  #define Protocol_Header   0x5E
  #define Protocol_Tail      0x5E
  
  
  // Data Ids  (bp = before point; af = after point)
  // Official data IDs
  #define ID_GPS_altidute_bp    0x01
  #define ID_GPS_altidute_ap    0x09
  #define ID_Temprature1        0x02
  #define ID_RPM                0x03
  #define ID_Fuel_level         0x04
  #define ID_Temprature2        0x05
  #define ID_Volt               0x06
  #define ID_Altitude_bp        0x10
  #define ID_Altitude_ap        0x21
  #define ID_GPS_speed_bp       0x11
  #define ID_GPS_speed_ap       0x19
  #define ID_Longitude_bp       0x12
  #define ID_Longitude_ap       0x1A
  #define ID_E_W                0x22
  #define ID_Latitude_bp        0x13
  #define ID_Latitude_ap        0x1B
  #define ID_N_S                0x23
  #define ID_Course_bp          0x14
  #define ID_Course_ap          0x24
  #define ID_Date_Month         0x15
  #define ID_Year               0x16
  #define ID_Hour_Minute        0x17
  #define ID_Second             0x18
  #define ID_Acc_X              0x24
  #define ID_Acc_Y              0x25
  #define ID_Acc_Z              0x26
  #define ID_Voltage_Amp_bp     0x3A
  #define ID_Voltage_Amp_ap     0x3B
  #define ID_Current            0x28
  // User defined data IDs
  #define ID_Gyro_X             0x40
  #define ID_Gyro_Y             0x41
  #define ID_Gyro_Z             0x42

   // Main function FrSky telemetry
    void telemetry_frsky() 
   {         
      if (currentTime > FrSkyTime ) // 
      {          
         FrSkyTime = currentTime + Time_telemetry_send;
         cycleCounter++;
         // Datas sent every 125 ms
            send_Accel();
            sendDataTail();   

         if ((cycleCounter % 4) == 0) 
         {      
            // Datas sent every 500ms
            send_Altitude();
            send_RPM();
            send_Course();
            send_GPS_speed();
            //send_Cell_volt();       todo
            sendDataTail();   

         }
         if ((cycleCounter % 8) == 0) 
         {      
            // Datas sent every 1s            
            //send_Time();
            send_GPS_position();
            send_GPS_altitude();
            send_Temperature2();  // Distance_to_home
            //send_Fuel_level();
            send_Voltage_ampere();
            send_Temperature1();  // num of Sats
            sendDataTail();            
         }

         if (cycleCounter == 40) 
         {
            // Datas sent every 5s
            cycleCounter = 0;       
         }
      }
   }

   void write_FrSky8(uint8_t Data)
   {
      SerialWrite(TELEMETRY_FRSKY_SERIAL, Data);
   }

   void write_FrSky16(uint16_t Data)
   {
      uint8_t Data_send;
      Data_send = Data;      
      check_FrSky_stuffing(Data_send);
      Data_send = Data >> 8 & 0xff;
      check_FrSky_stuffing(Data_send);
   }
   
   void check_FrSky_stuffing(uint8_t Data) //byte stuffing
   {
      if (Data == 0x5E)   
      {
         write_FrSky8(0x5D);
         write_FrSky8(0x3E);
      }
      else if (Data == 0x5D)   
      {
         write_FrSky8(0x5D);
         write_FrSky8(0x3D);
      }
      else
      {
         write_FrSky8(Data);         
      }
   }

   static void sendDataHead(uint8_t Data_id)
   {
      write_FrSky8(Protocol_Header);
      write_FrSky8(Data_id);
   }

   static void sendDataTail(void)
   {
      write_FrSky8(Protocol_Tail);      
   }


   //*********************************************************************************
   //-----------------   Telemetrie Datas   ------------------------------------------   
   //*********************************************************************************

   // GPS altitude
   void send_GPS_altitude(void)
   {          
      if (f.GPS_FIX && GPS_numSat >= 4) 
      {
         int16_t Datas_GPS_altidute_bp;
         uint16_t Datas_GPS_altidute_ap;

         Datas_GPS_altidute_bp = GPS_altitude;
         Datas_GPS_altidute_ap = 0;

         sendDataHead(ID_GPS_altidute_bp);
         write_FrSky16(Datas_GPS_altidute_bp);
         sendDataHead(ID_GPS_altidute_ap);
         write_FrSky16(Datas_GPS_altidute_ap);
      }
   }
   
   // Temperature
   void send_Temperature1(void)
   {
      int16_t Datas_Temprature1;

      Datas_Temprature1 = GPS_numSat;  // Number of Satalits alias Temp1

      sendDataHead(ID_Temprature1);
      write_FrSky16(Datas_Temprature1); 
   }

   // RPM
   void send_RPM(void)
   {
      uint16_t Datas_RPM = 0;      
      for (uint8_t i=0;i<NUMBER_MOTOR;i++)
      {
         Datas_RPM += motor[i];
      } 
      Datas_RPM = (Datas_RPM / NUMBER_MOTOR) / 30;   // RPM 

      sendDataHead(ID_RPM);
      write_FrSky16(Datas_RPM);
   }

   // Fuel level
   void send_Fuel_level(void)
   {
      uint16_t Datas_Fuel_level;

      Datas_Fuel_level = 0; 

      sendDataHead(ID_Fuel_level);
      write_FrSky16(Datas_Fuel_level); 
   }

   // Temperature 2
   void send_Temperature2(void)
   {
      if (f.GPS_FIX_HOME)
      {
         int16_t Datas_Temprature2;

         Datas_Temprature2 = GPS_distanceToHome; // Distance to home alias Temp2

         sendDataHead(ID_Temprature2);
         write_FrSky16(Datas_Temprature2);  
      }      
   }

   // Cell voltage  todo !!!!!!!!!!!!!!!!!!
   void send_Cell_volt(void) // Datas FrSky FLVS-01 voltage sensor
   {
      uint16_t Datas_Volt;
      uint8_t number_of_cells = 0;   // LiPo 3S = 3; LiPo 4S = 4 ...
      static uint8_t cell = 0;
      if (cell >= number_of_cells); cell = 0;
      
      Datas_Volt = 0; // 0.01v / 0 ~ 4.2v

      sendDataHead(ID_Volt);
      write_FrSky16(Datas_Volt); 
   }

   // Altitude
   void send_Altitude(void)
   {
      uint16_t Datas_altitude_bp;
      uint16_t Datas_altitude_ap;
      static uint16_t Start_altitude;

      if (!f.ARMED)
      {
         Start_altitude = EstAlt / 100;
      }

      Datas_altitude_bp = (EstAlt / 100) - Start_altitude;
      Datas_altitude_ap = (EstAlt % 100);

      sendDataHead(ID_Altitude_bp);
      write_FrSky16(Datas_altitude_bp);
      sendDataHead(ID_Altitude_ap);
      write_FrSky16(Datas_altitude_ap);
   }

   // GPS speed
   void send_GPS_speed(void)
   {
      if (f.GPS_FIX && GPS_numSat >= 4) 
      {            
         uint16_t Datas_GPS_speed_bp;
         uint16_t Datas_GPS_speed_ap;
         
         Datas_GPS_speed_bp = GPS_speed * 0.036;
         Datas_GPS_speed_ap = 0;
      
         sendDataHead(ID_GPS_speed_bp);
         write_FrSky16(Datas_GPS_speed_bp);
         sendDataHead(ID_GPS_speed_ap);
         write_FrSky16(Datas_GPS_speed_ap);
      }
   }

   // GPS position
   void send_GPS_position(void)
   {
   
         uint16_t Datas_Longitude_bp;
         uint16_t Datas_Longitude_ap;
         uint16_t Datas_E_W;
         uint16_t Datas_Latitude_bp;
         uint16_t Datas_Latitude_ap;
         uint16_t Datas_N_S;
         Datas_Longitude_bp = abs(GPS_coord[LON]) / 100000;
         Datas_Longitude_ap = abs((GPS_coord[LON])/10)  % 10000;
         Datas_E_W = GPS_coord[LON] < 0 ? 'W' : 'E';
         Datas_Latitude_bp = abs(GPS_coord[LAT]) / 100000;
         Datas_Latitude_ap = abs((GPS_coord[LAT])/10) % 10000;
         Datas_N_S = GPS_coord[LAT] < 0 ? 'S' : 'N';

         sendDataHead(ID_Longitude_bp);
         write_FrSky16(Datas_Longitude_bp);
         sendDataHead(ID_Longitude_ap);
         write_FrSky16(Datas_Longitude_ap);
         sendDataHead(ID_E_W);
         write_FrSky16(Datas_E_W);

         sendDataHead(ID_Latitude_bp);
         write_FrSky16(Datas_Latitude_bp);
         sendDataHead(ID_Latitude_ap);
         write_FrSky16(Datas_Latitude_ap);
         sendDataHead(ID_N_S);
         write_FrSky16(Datas_N_S);
      
   }

   // Course
   void send_Course(void)
   {
      uint16_t Datas_Course_bp;
      uint16_t Datas_Course_ap;

      Datas_Course_bp = heading;
      Datas_Course_ap = 0;

      sendDataHead(ID_Course_bp);
      write_FrSky16(Datas_Course_bp);
      sendDataHead(ID_Course_ap);
      write_FrSky16(Datas_Course_ap);
   }

   // Time
   void send_Time(void)
   {
      uint32_t seconds_since_start = millis() / 1000;
            
      uint16_t Datas_Date_month;
      uint16_t Datas_Year;
      uint16_t Datas_Minutes_hours;
      uint16_t Datas_seconds;
            
      Datas_Date_month = 0;
      Datas_Year = 12;
      Datas_Minutes_hours = (seconds_since_start / 60) % 60;
      Datas_seconds = seconds_since_start % 60;      
      
      sendDataHead(ID_Hour_Minute);
      write_FrSky16(Datas_Minutes_hours);
      sendDataHead(ID_Second);
      write_FrSky16(Datas_seconds);
   }

   // ACC
   void send_Accel(void)
   {
      int16_t Datas_Acc_X;
      int16_t Datas_Acc_Y;
      int16_t Datas_Acc_Z;

      Datas_Acc_X = ((float)accSmooth[0] / acc_1G) * 1000;
      Datas_Acc_Y = ((float)accSmooth[1] / acc_1G) * 1000;
      Datas_Acc_Z = ((float)accSmooth[2] / acc_1G) * 1000;

      sendDataHead(ID_Acc_X);
      write_FrSky16(Datas_Acc_X);
      sendDataHead(ID_Acc_Y);
      write_FrSky16(Datas_Acc_Y);
      sendDataHead(ID_Acc_Z);
      write_FrSky16(Datas_Acc_Z);      
   }

   // Voltage (Ampere Sensor)  
   void send_Voltage_ampere(void)
   {
   
      #if defined (FAS_100)   // todo   !!!!!!!!!!!!!!!!!
      {
         uint16_t Datas_Voltage_Amp_bp;
         uint16_t Datas_Voltage_Amp_ap;
         uint16_t Datas_Current;   

         Datas_Voltage_Amp_bp = 0;
         Datas_Voltage_Amp_ap = 0;   
         Datas_Current = 0;

         sendDataHead(ID_Voltage_Amp_bp);
         write_FrSky16(Datas_Voltage_Amp_bp);
         sendDataHead(ID_Voltage_Amp_ap);
         write_FrSky16(Datas_Voltage_Amp_ap);   
         sendDataHead(ID_Current);
         write_FrSky16(Datas_Current);
      }
      #else   // use vBat
      {
         uint16_t Datas_Voltage_vBat_bp;
         uint16_t Datas_Voltage_vBat_ap;   
         uint16_t voltage;
         voltage = (vbat * 110) / 21;          
         Datas_Voltage_vBat_bp = voltage / 100;
         Datas_Voltage_vBat_ap = ((voltage % 100) + 5) / 10;         

         sendDataHead(ID_Voltage_Amp_bp);
         write_FrSky16(Datas_Voltage_vBat_bp);
         sendDataHead(ID_Voltage_Amp_ap);
         write_FrSky16(Datas_Voltage_vBat_ap);   
      }
      #endif
   }

#endif

Init isn't needed anymore, but you need to place this somewhere in the annexCode() function:

Code: Select all

  #if defined(TELEMETRY_FRSKY) // new
   telemetry_frsky();
  #endif

Last, but not least in config.h ... add/change:

Code: Select all

#define TELEMETRY_FRSKY

    #define SERIAL0_COM_SPEED 115200
    #define SERIAL1_COM_SPEED 9600 // it's on serial 1 for me ... so set this to 9600 baud 
    #define SERIAL2_COM_SPEED 115200
    #define SERIAL3_COM_SPEED 115200

That's it ... most of the functions in FrSkyTelemetry.ino don't send useful information yet. Feel free to change that ... I'd like to see a more complete version of this included in MultiWii ;-)

This changes from Sebbi are working just great as told before with r1240, any news on this about the usefull info, and about integration on the MWC? I think that this downlink have potential...

Cheers

vpb · Post by **vpb** » Thu Nov 15, 2012 11:29 am

i use it with r1268 without any problem.

mysku82 · Post by **mysku82** » Tue Nov 27, 2012 12:12 am

I can send someone ready assembled code. I tried to assemble it but I do not show anything on the station and I want to remove the software problem.
PS : We solved. The code was ok. I used MAX232 and I do not connect well with him.
Now where it can bind FAS-100 or FLVS-01
Thank you

kataventos · Post by **kataventos** » Sat Dec 15, 2012 6:10 pm

Can we get telemetry info on the tx pin of the receiver, since we are just using RX?
I was thinking on getting digital data like the RSSI and other readings into the FC, without using PPMSUM

R_Lefebvre · Post by **R_Lefebvre** » Mon Jan 07, 2013 11:30 pm

Hi guys, I'm a dev on Arducopter. Do you mind if I use this code? Who do I credit?

monkey · Post by **monkey** » Thu Jan 10, 2013 5:15 pm

R_Lefebvre wrote:Hi guys, I'm a dev on Arducopter. Do you mind if I use this code? Who do I credit?

It looks like this has been a serious group effort with a few key producers. I would say the best bet is to link to this forum run. ie.

Code credit to those contributors here : "Link to forum"

Just an idea, I had no parts in any of this, and am more than grateful you guys have been working on this, I am waiting on a few parts to make it work and look forward to trying to use your code. Thank you for all those posting and contributing to this.

I too have a flyduino mega and cant wait to give this info a try.

Any ideas if this will be implemented in next release of multiwii (2.2)?

larsm · Post by **larsm** » Sun Jan 13, 2013 4:05 pm

I wrote some instructions to do the mod: klick

imcoref · Post by **imcoref** » Sun Jan 13, 2013 4:13 pm

will check it out! and thank you very much!

Danke!

monkey · Post by **monkey** » Wed Jan 23, 2013 8:37 pm

Thank you larsm for the write up, it was a little hard for me to follow but the 2nd or 3rd time I read it I got it worked out.

Here is my setup:

I got two mini rs232 to ttl adapters off ebay http://www.ebay.com/itm/251103811631?ssPageName=STRK:MEWNX:IT&_trksid=p3984.m1497.l2649 one used to hookup serial 3 on flyduino mega to D8R-XP frsky rx and one is used on Frsky DJT module to the bluetooth atapter, (I am using a 9x with er9x firmware)

I got a Sirus Bluetooth Adapter off ebay http://www.ebay.com/itm/MWC-Multiwii-SE-Flight-Control-Bluetooth-Parameter-Debug-Module-Adapter-/261138423472?_trksid=p5197.m1992&_trkparms=aid%3D111000%26algo%3DREC.CURRENT%26ao%3D1%26asc%3D14%26meid%3D5084054663530866329%26pid%3D100015%26prg%3D1006%26rk%3D1%26sd%3D261138423472%26
It does work but I had to use Arduino Serial Monitor and AT Commands to change the default baud rate to 9600.

I am using multwii Ez-Gui on my android phone to display the info. It all seems to be working great. The only issue I have, is that the longitude shows a Positive value and where I am at it should be negative, so I can not use the GUI map. Anyone know how to correct this?

If I connect to multiwii config on my PC it shows the GPS value correctly, so I know its not the multiwii software or the GPS module, unless there is something the FrskyTelemetry.ino.

copterrichie · Post by **copterrichie** » Wed Jan 23, 2013 8:45 pm

Question: Is it possible to use the Direct Telemetry data with a 433mhz Transceiver?

monkey · Post by **monkey** » Wed Jan 23, 2013 8:58 pm

Yes on a serial port. Here is the kit I have:
http://rctimer.com/index.php?gOo=goods_details.dwt&goodsid=834&productname=

They also have a 915mhz kit.

Naturally this would done with Multiwii Confic, or Multiwii WinGUI on a PC, however it should be hard to adapt and program these to work together and transmit data to a bluetooth module..

copterrichie · Post by **copterrichie** » Wed Jan 23, 2013 9:25 pm

Awesome and thank you.

monkey · Post by **monkey** » Wed Jan 23, 2013 9:41 pm

copterrichie wrote:Awesome and thank you.

No problem, also ment to note that it is 2-way communication, so you can program with Arduino or multwii config as well. Sometimes I use the kit just to hook up and benchtest everything without having a cable going to my pc, makes calibration test and checking my power system setup much easier...

I may look at hooking up the USB side, to my bluetooth module that way I can program and get telemetry to multiwii EZ-GUI out in the field without the need of a computer.

copterrichie · Post by **copterrichie** » Thu Jan 24, 2013 3:55 am

Did this patch/code make it into one of the development release? I looked for it and could not find it in the R1311 release.

monkey · Post by **monkey** » Thu Jan 24, 2013 4:16 am

No the code is not in R1311, I had to add it in.

Sebbi · Post by **Sebbi** » Mon Jan 28, 2013 4:17 pm

Anyone interested in a more complete version with support for using a pin to directly connect to the FrSky receiver (inverted logic / soft serial)?

copterrichie · Post by **copterrichie** » Mon Jan 28, 2013 4:19 pm

Make it universal and not just specific for Frsky, Heck ya!

Urbasik · Post by **Urbasik** » Mon Jan 28, 2013 4:27 pm

Tried it yesterday with a simple lvl converter/inverter and it works fine so far.
Soft Serial would be nice for for 2.2.

IceWind · Post by **IceWind** » Mon Jan 28, 2013 5:13 pm

I suspect you mean that for the MEGA's not for the ProMini's right?

copterrichie · Post by **copterrichie** » Mon Jan 28, 2013 5:20 pm

The Mega would not require a Soft Serial. heads up, the Arduino Softserial library does not work well with MWC, don't ask me how I know. :eek:

IceWind · Post by **IceWind** » Mon Jan 28, 2013 5:25 pm

Sebbi · Post by **Sebbi** » Mon Jan 28, 2013 6:21 pm

copterrichie wrote:Make it universal and not just specific for Frsky, Heck ya!

And how would one do that? There is already LCD telemetry within MultiWii and FrSky telemetry is nothing like it ... Graupners Hott however, might be another story, but I don't have the hardware required to test this.

IceWind wrote:I suspect you mean that for the MEGA's not for the ProMini's right?

MEGA as well as ProMini or Leonardo. The TX-part of SoftSerial is very small and doesn't take too many bytes. It increases the cycle time, however.

copterrichie wrote:The Mega would not require a Soft Serial. heads up, the Arduino Softserial library does not work well with MWC, don't ask me how I know.

Sending with SoftSerial does work fine and I stripped the required code from that library, so it works for FrSky telemetry.

And including SoftSerial only requires additional 332 bytes ;-)

copterrichie · Post by **copterrichie** » Mon Jan 28, 2013 6:25 pm

Care to share the stripped down version of the SoftSerial Library? I have an need for it right now.

Sebbi · Post by **Sebbi** » Mon Jan 28, 2013 6:32 pm

copterrichie wrote:Care to share the stripped down version of the SoftSerial Library? I have an need for it right now.

I hope you find the needed parts in this code extract. initTelemetry() has to be called in setup() and write_softserial() writes a single byte on the configured pin with the desired baudrate (which is 9600 in case of FrSky). I attached the complete lookup tables below the code, so you can change it for whatever baudrate you need. And remember ... this is transmitting only and uses inverted logic. You have to switch the cases in tx_pin_write() to get normal serial logic.

Code: Select all

  #define TELEMETRY_FRSKY_PIN 4

  #if defined(TELEMETRY_FRSKY_PIN)
    uint8_t frsky_transmitBitMask;
    volatile uint8_t *frsky_transmitPortRegister;  
    
    #define TELEMETRY_FRSKY_WRITE(x) write_softserial(x)
    
    #if F_CPU == 16000000
    uint16_t frsky_tx_delay = 233;     // value from original SoftSerial for baudrate 9600
    const int frsky_XMIT_START_ADJUSTMENT = 5;
    
    #elif F_CPU == 8000000
    uint16_t frsky_tx_delay = 112;     // value from original SoftSerial for baudrate 9600
    const int frsky_XMIT_START_ADJUSTMENT = 4;
    
    #elif F_CPU == 20000000
    uint16_t frsky_tx_delay = 294;     // value from original SoftSerial for baudrate 9600
    const int frsky_XMIT_START_ADJUSTMENT = 6;
    
    #else
    #error SoftSerial (FrSky) supports only 20, 16 and 8MHz processors
    #endif
    
  #elif defined(TELEMETRY_FRSKY_SERIAL)
    #define TELEMETRY_FRSKY_WRITE(x) SerialWrite(TELEMETRY_FRSKY_SERIAL, x);
    
  #else
  #error FrSky telemetry requires either a pin (SoftSerial) or a real serial port being set in config.h
  #endif  
  
  #if defined(TELEMETRY_FRSKY_WRITE)
  
  void initTelemetry()
  {
    #if defined(TELEMETRY_FRSKY_PIN)
    // set pin to output and prepare bitmask
    pinMode(TELEMETRY_FRSKY_PIN, OUTPUT);
    digitalWrite(TELEMETRY_FRSKY_PIN, HIGH);
    frsky_transmitBitMask = digitalPinToBitMask(TELEMETRY_FRSKY_PIN);
    uint8_t port = digitalPinToPort(TELEMETRY_FRSKY_PIN);
    frsky_transmitPortRegister = portOutputRegister(port);

    #endif
    // initialisation when using serial port is done automatically
  }

  #if defined(TELEMETRY_FRSKY_PIN)
  void tx_pin_write(uint8_t pin_state)
  {
    if (pin_state == LOW)
      *frsky_transmitPortRegister &= ~frsky_transmitBitMask;
    else
      *frsky_transmitPortRegister |= frsky_transmitBitMask;
  }

  inline void tunedDelay(uint16_t delay) { 
    uint8_t tmp=0;
  
    // +w instead of +r, because gcc complained
    asm volatile("sbiw    %0, 0x01 \n\t"
      "ldi %1, 0xFF \n\t"
      "cpi %A0, 0xFF \n\t"
      "cpc %B0, %1 \n\t"
      "brne .-10 \n\t"
      : "+w" (delay), "+a" (tmp)
      : "0" (delay)
      );
  }

  void write_softserial(uint8_t b) {
    uint8_t oldSREG = SREG;
    cli();  // turn off interrupts for a clean txmit

    // Write the start bit
    tx_pin_write(HIGH);
    tunedDelay(frsky_tx_delay + frsky_XMIT_START_ADJUSTMENT);    

    // Write each of the 8 bits
    for (byte mask = 0x01; mask; mask <<= 1)
    {
      if (b & mask) // choose bit
        tx_pin_write(LOW); // send 1
      else
        tx_pin_write(HIGH); // send 0
    
      tunedDelay(frsky_tx_delay);
    }

    tx_pin_write(LOW); // restore pin to natural state
  
    SREG = oldSREG; // turn interrupts back on
    tunedDelay(frsky_tx_delay);    
  }
  #endif

Code: Select all

//
// Lookup table
//
typedef struct _DELAY_TABLE
{
  long baud;
  unsigned short rx_delay_centering;
  unsigned short rx_delay_intrabit;
  unsigned short rx_delay_stopbit;
  unsigned short tx_delay;
} DELAY_TABLE;

#if F_CPU == 16000000

static const DELAY_TABLE PROGMEM table[] = 
{
  //  baud    rxcenter   rxintra    rxstop    tx
  { 115200,   1,         17,        17,       12,    },
  { 57600,    10,        37,        37,       33,    },
  { 38400,    25,        57,        57,       54,    },
  { 31250,    31,        70,        70,       68,    },
  { 28800,    34,        77,        77,       74,    },
  { 19200,    54,        117,       117,      114,   },
  { 14400,    74,        156,       156,      153,   },
  { 9600,     114,       236,       236,      233,   },
  { 4800,     233,       474,       474,      471,   },
  { 2400,     471,       950,       950,      947,   },
  { 1200,     947,       1902,      1902,     1899,  },
  { 300,      3804,      7617,      7617,     7614,  },
};

const int XMIT_START_ADJUSTMENT = 5;

#elif F_CPU == 8000000

static const DELAY_TABLE table[] PROGMEM = 
{
  //  baud    rxcenter    rxintra    rxstop  tx
  { 115200,   1,          5,         5,      3,      },
  { 57600,    1,          15,        15,     13,     },
  { 38400,    2,          25,        26,     23,     },
  { 31250,    7,          32,        33,     29,     },
  { 28800,    11,         35,        35,     32,     },
  { 19200,    20,         55,        55,     52,     },
  { 14400,    30,         75,        75,     72,     },
  { 9600,     50,         114,       114,    112,    },
  { 4800,     110,        233,       233,    230,    },
  { 2400,     229,        472,       472,    469,    },
  { 1200,     467,        948,       948,    945,    },
  { 300,      1895,       3805,      3805,   3802,   },
};

const int XMIT_START_ADJUSTMENT = 4;

#elif F_CPU == 20000000

// 20MHz support courtesy of the good people at macegr.com.
// Thanks, Garrett!

static const DELAY_TABLE PROGMEM table[] =
{
  //  baud    rxcenter    rxintra    rxstop  tx
  { 115200,   3,          21,        21,     18,     },
  { 57600,    20,         43,        43,     41,     },
  { 38400,    37,         73,        73,     70,     },
  { 31250,    45,         89,        89,     88,     },
  { 28800,    46,         98,        98,     95,     },
  { 19200,    71,         148,       148,    145,    },
  { 14400,    96,         197,       197,    194,    },
  { 9600,     146,        297,       297,    294,    },
  { 4800,     296,        595,       595,    592,    },
  { 2400,     592,        1189,      1189,   1186,   },
  { 1200,     1187,       2379,      2379,   2376,   },
  { 300,      4759,       9523,      9523,   9520,   },
};

const int XMIT_START_ADJUSTMENT = 6;

#else

#error This version of SoftwareSerial supports only 20, 16 and 8MHz processors

#endif

IceWind · Post by **IceWind** » Mon Jan 28, 2013 6:34 pm

Sebbi wrote:
IceWind wrote:I suspect you mean that for the MEGA's not for the ProMini's right?

MEGA as well as ProMini or Leonardo. The TX-part of SoftSerial is very small and doesn't take too many bytes. It increases the cycle time, however.

Perfect. No problem in my case (unless the increase is huge) as I always have low cycle times due to never using anything else other than gyros.
The use case is to get GPS data, as I'll be using the I2C converter again the cycle time shouldn't suffer much.

copterrichie · Post by **copterrichie** » Mon Jan 28, 2013 6:37 pm

Thank you

QuadBow · Post by **QuadBow** » Sat Feb 02, 2013 8:04 am

Hi guys,

I got one question as to FRSKY telemetry since the voltage stuff is still unclear to me.

Understanding that
ID_Volt 0x06
is related to FLVS-01 voltage sensor covering cells' voltage
I wonder if
ID_Voltage_Amp_bp 0x3A
and
ID_Voltage_Amp_ap 0x3B
are the same the built-in voltages A1 and A2.
Will they overwrite A1/A2 or are there more IDs for A1/A2 which are independend?

The Frsky documentation of the FLD-02 mentions a "rate", which is not included in the transfer protocol list.
Does anybody know more?

subaru4wd · Post by **subaru4wd** » Sun Feb 10, 2013 6:30 pm

larsm wrote:I wrote some instructions to do the mod: klick

This link no longer works.

Does anybody have a guide to this mod? I just ordered the converter & I am not sure where it needs to be installed.

QuadBow · Post by **QuadBow** » Sun Feb 17, 2013 9:13 am

Yesterday, I worked a bit on the FRSKY telemetry integration into my multiwii configuration including the telemetry display FLD-02.
Thanks to the great job of others most of the functionalitys work already well.
In the current implementation rpm lies within the range of 1000 to 2000 in accordance to the multiwii motor[i] variable.
I wrote a small change of code in order to present the "real" rpm values.

void inline send_RPM(void)
{
uint16_t Data_RPM = 0;
float rpm;

for (uint8_t i=0; i<NUMBER_MOTOR; i++)
{
Data_RPM += motor[i] - MINCOMMAND;
}
#if defined(VBAT)
rpm = ((float) Data_RPM * (float) vbat * KV_MOTOR) / ((float) NUMBER_MOTOR * 300.0 * (float) (MAXTHROTTLE - MINCOMMAND));
#else
rpm = ((float) Data_RPM * 120.0 * KV_MOTOR) / ((float) NUMBER_MOTOR * 300.0 * (float) (MAXTHROTTLE - MINCOMMAND));
#endif
Data_RPM = (uint16_t) rpm;

sendDataHead(ID_RPM);
write_FrSky16(Data_RPM);
}

This is not a real measurement, but an estimation of the rpm. You have to define in config.h
#define KV_MOTOR 1500.0 // kv of motors in rpm/V
for motors which rotate with a kv-value of 1500.0 rpm per volt.
You should enable VBAT as well, the algorithm assumes a steady voltage of 12.0 V, which is not that accurate.
The rpm-value is divided by 30 (due to 300.0) in order to fit in the FLD-02 scheme.

Does anybody have other telemetry displays (e.g. telemetry integration into the turnigy 9x display) in order to make the code suitable for other configurations and therefore more universal?

QuadBow · Post by **QuadBow** » Tue Feb 19, 2013 6:25 pm

I found some issues within the already provided file "telemetry.ino" and fixed them for my purposes.

1. The altitude of baro based estimation accepts only one 16-bit number (before point). The after point part doesn't have any effect, so a abolished it.

void inline send_Altitude(void)
{
uint16_t Data_altitude;
static uint16_t Start_altitude;

if (!f.ARMED)
Start_altitude = EstAlt / 100;
Data_altitude = EstAlt / 100 - Start_altitude;

sendDataHead(ID_Altitude);
write_FrSky16(Data_altitude);
}

2. The Frsky telemetry protocol expects the speed in knots, not in kilometers per hour. I could confirm this expectation by testing.
Thus, I had to change the factor 0.036 (cm/s to km/h) to 0.019726 (cm/s to kn). Furthermore, I implemented the after point part for a better accuracy.

void send_GPS_speed(void)
{
if (f.GPS_FIX && GPS_numSat >= 4)
{
uint16_t temp;
uint16_t Datas_GPS_speed_bp;
uint16_t Datas_GPS_speed_ap;

temp = GPS_speed * 0.19726;
Datas_GPS_speed_bp = temp / 10;
Datas_GPS_speed_ap = temp % 10;

sendDataHead(ID_GPS_speed_bp);
write_FrSky16(Datas_GPS_speed_bp);
sendDataHead(ID_GPS_speed_ap);
write_FrSky16(Datas_GPS_speed_ap);
}
}

The fixed issues are related to FLD-02 v2.1. Are those issues also known for other devices / firmware versions?
I would be grateful receiving more information about other display devices in order to support the development of a Frsky telemetry integration to a more universal extend.

Action · Post by **Action** » Tue Mar 26, 2013 12:45 pm

Hello all, I am kinda new to RC hobby and it looks like I have came in at a great time! I was looking around for the perfect radio system for the tricopter that I am about to build and came across this thread. I am currently in college taking computer programming classes, and hopefully I will be able to contribute to the community.

I am waiting on all my parts to come it right now to get started. I have the HK multiwii 328p on order for the flight control and a serial to bluetooth module to upgrade and make changes.

I was looking at getting a Turnigy 9x when I came across the new FrSky Taranis being discussed. It is supposed to be out in April, not sure of the price yet, but it has great potential. It has the FrSky Telemetry built in and has up to 16 channels. Nice backlit screen, plenty of switches and such. It has the open9x installed and can transmit user data over serial bus.
I think the addition of the code found in this thread along with this new radio will be a game changer, very exciting stuff.

We could probably connect a serial bluetooth module on the tranciever and be able to transfer data to the board through the receiver.

Hopefully the new controller uses the same method to broadcast the telemetry data as has been discusse here.

kataventos · Post by **kataventos** » Tue Apr 23, 2013 10:57 pm

@Quadbow,

nice of you to take some time with this and continue the developments.
I looked to your code lines and I will try asap the RPM mod

it sure looks nice. About speed, nice for the knots guys

on the altitude do not see problem on using what is done so far besides user´s that want to make this mod will need to change as follows either:

Code: Select all

  #define ID_Altitude_bp        0x10
  #define ID_Altitude_ap        0x21

to

Code: Select all

  #define ID_Altitude        0x10
  //#define ID_Altitude_ap        0x21

Anyway, some more changes have to be done in order to have it running with MWii 2.2 but they are easy to catch by the user.
About your question, I do have the 9X with ER9X and it is working just fine

Cheers,
KV

mysku82 · Post by **mysku82** » Thu Apr 25, 2013 9:58 pm

It gives me error loading the r1391. Does anyone know why?

FrSkyTelemetry.ino: In function 'void send_Altitude()':
FrSkyTelemetry:251: error: 'EstAlt' was not declared in this scope
FrSkyTelemetry:254: error: 'EstAlt' was not declared in this scope
FrSkyTelemetry.ino: In function 'void send_Course()':
FrSkyTelemetry:320: error: 'heading' was not declared in this scope
FrSkyTelemetry.ino: In function 'void send_Accel()':
FrSkyTelemetry:357: error: 'accSmooth' was not declared in this scope
FrSkyTelemetry:357: error: 'acc_1G' was not declared in this scope
FrSkyTelemetry.ino: In function 'void send_Voltage_ampere()':
FrSkyTelemetry:395: error: 'vbat' was not declared in this scope

Thanks

Best Regards
Cosmin Miscu

kataventos · Post by **kataventos** » Thu Apr 25, 2013 11:38 pm

mysku82 wrote:It gives me error loading the r1391. Does anyone know why?

Hi Cosmin,

what I said on my last post, they are easy to catch. Look at the MWii code and change on the FRSky_telemetry sketch accordingly.

Cheers,
KV

kataventos · Post by **kataventos** » Fri Apr 26, 2013 1:01 am

OK got a few minutes into it, try this entire new sketch:

FrSky_telemetry

Code: Select all

// ****************************************************************
// FrSky telemetry
// Version: 0.2.2
// Date 26/04/2013
// KataVentos Changes: V0.2.2: - made it work with 2.2 (r1391) plus Quadbow RPM mod.
// ****************************************************************

#if defined(TELEMETRY_FRSKY)
  // user defines
  #define FAS_100  //if commment out, MultiWii vBat voltage will be send instead of FrSky FAS 100 voltage


  // Serial config datas
  #define TELEMETRY_FRSKY_SERIAL 3
  #define TELEMETRY_FRSKY_BAUD 9600  

  // Timing
  #define Time_telemetry_send 125000 
  static uint8_t cycleCounter = 0;
  static uint32_t FrSkyTime  = 0;

  // Frame protocol
  #define Protocol_Header   0x5E
  #define Protocol_Tail      0x5E
  
  
  // Data Ids  (bp = before point; af = after point)
  // Official data IDs
  #define ID_GPS_altidute_bp    0x01
  #define ID_GPS_altidute_ap    0x09
  #define ID_Temprature1        0x02
  #define ID_RPM                0x03
  #define ID_Fuel_level         0x04
  #define ID_Temprature2        0x05
  #define ID_Volt               0x06
  #define ID_Altitude_bp        0x10
  #define ID_Altitude_ap        0x21
  #define ID_GPS_speed_bp       0x11
  #define ID_GPS_speed_ap       0x19
  #define ID_Longitude_bp       0x12
  #define ID_Longitude_ap       0x1A
  #define ID_E_W                0x22
  #define ID_Latitude_bp        0x13
  #define ID_Latitude_ap        0x1B
  #define ID_N_S                0x23
  #define ID_Course_bp          0x14
  #define ID_Course_ap          0x24
  #define ID_Date_Month         0x15
  #define ID_Year               0x16
  #define ID_Hour_Minute        0x17
  #define ID_Second             0x18
  #define ID_Acc_X              0x24
  #define ID_Acc_Y              0x25
  #define ID_Acc_Z              0x26
  #define ID_Voltage_Amp_bp     0x3A
  #define ID_Voltage_Amp_ap     0x3B
  #define ID_Current            0x28
  // User defined data IDs
  #define ID_Gyro_X             0x40
  #define ID_Gyro_Y             0x41
  #define ID_Gyro_Z             0x42

   // Main function FrSky telemetry
    void telemetry_frsky() 
   {         
      if (currentTime > FrSkyTime ) // 
      {          
         FrSkyTime = currentTime + Time_telemetry_send;
         cycleCounter++;
         // Datas sent every 125 ms
            //send_Accel();
            sendDataTail();   

         if ((cycleCounter % 4) == 0) 
         {      
            // Datas sent every 500ms
            send_Altitude();
            send_RPM();
            send_Course();
            send_GPS_speed();
            //send_Cell_volt();       todo
            sendDataTail();   

         }
         if ((cycleCounter % 8) == 0) 
         {      
            // Datas sent every 1s            
            //send_Time();
            send_GPS_position();
            send_GPS_altitude();
            send_Temperature2();  // Distance_to_home
            //send_Fuel_level();
            send_Voltage_ampere();
            send_Temperature1();  // num of Sats
            sendDataTail();            
         }

         if (cycleCounter == 40) 
         {
            // Datas sent every 5s
            cycleCounter = 0;       
         }
      }
   }

   void write_FrSky8(uint8_t Data)
   {
      SerialWrite(TELEMETRY_FRSKY_SERIAL, Data);
   }

   void write_FrSky16(uint16_t Data)
   {
      uint8_t Data_send;
      Data_send = Data;      
      check_FrSky_stuffing(Data_send);
      Data_send = Data >> 8 & 0xff;
      check_FrSky_stuffing(Data_send);
   }
   
   void check_FrSky_stuffing(uint8_t Data) //byte stuffing
   {
      if (Data == 0x5E)   
      {
         write_FrSky8(0x5D);
         write_FrSky8(0x3E);
      }
      else if (Data == 0x5D)   
      {
         write_FrSky8(0x5D);
         write_FrSky8(0x3D);
      }
      else
      {
         write_FrSky8(Data);         
      }
   }

   static void sendDataHead(uint8_t Data_id)
   {
      write_FrSky8(Protocol_Header);
      write_FrSky8(Data_id);
   }

   static void sendDataTail(void)
   {
      write_FrSky8(Protocol_Tail);      
   }


   //*********************************************************************************
   //-----------------   Telemetrie Datas   ------------------------------------------   
   //*********************************************************************************

   // GPS altitude
   void send_GPS_altitude(void)
   {          
      if (f.GPS_FIX && GPS_numSat >= 4) 
      {
         int16_t Datas_GPS_altidute_bp;
         uint16_t Datas_GPS_altidute_ap;

         Datas_GPS_altidute_bp = GPS_altitude;
         Datas_GPS_altidute_ap = 0;

         sendDataHead(ID_GPS_altidute_bp);
         write_FrSky16(Datas_GPS_altidute_bp);
         sendDataHead(ID_GPS_altidute_ap);
         write_FrSky16(Datas_GPS_altidute_ap);
      }
   }
   
   // Temperature
   void send_Temperature1(void)
   {
      int16_t Datas_Temprature1;

      Datas_Temprature1 = GPS_numSat;  // Number of Satalits alias Temp1

      sendDataHead(ID_Temprature1);
      write_FrSky16(Datas_Temprature1); 
   }

   // RPM
   void inline send_RPM(void)
   {
       uint16_t Data_RPM = 0;
       float rpm; 

       for (uint8_t i=0; i<NUMBER_MOTOR; i++)
 {
       Data_RPM += motor[i] - MINCOMMAND;
}
       #if defined(VBAT)
       rpm = ((float) Data_RPM * (float) analog.vbat * KV_MOTOR) / ((float) NUMBER_MOTOR * 300.0 * (float) (MAXTHROTTLE - MINCOMMAND));
       #else
       rpm = ((float) Data_RPM * 120.0 * KV_MOTOR) / ((float) NUMBER_MOTOR * 300.0 * (float) (MAXTHROTTLE - MINCOMMAND));
       #endif
       Data_RPM = (uint16_t) rpm;

       sendDataHead(ID_RPM);
       write_FrSky16(Data_RPM);
}


   // Fuel level
   void send_Fuel_level(void)
   {
      uint16_t Datas_Fuel_level;

      Datas_Fuel_level = 0; 

      sendDataHead(ID_Fuel_level);
      write_FrSky16(Datas_Fuel_level); 
   }

   // Temperature 2
   void send_Temperature2(void)
   {
      if (f.GPS_FIX_HOME)
      {
         int16_t Datas_Temprature2;

         Datas_Temprature2 = GPS_distanceToHome; // Distance to home alias Temp2

         sendDataHead(ID_Temprature2);
         write_FrSky16(Datas_Temprature2);  
      }      
   }

   // Cell voltage  todo !!!!!!!!!!!!!!!!!!
   void send_Cell_volt(void) // Datas FrSky FLVS-01 voltage sensor
   {
      uint16_t Datas_Volt;
      uint8_t number_of_cells = 0;   // LiPo 3S = 3; LiPo 4S = 4 ...
      static uint8_t cell = 0;
      if (cell >= number_of_cells); cell = 0;
      
      Datas_Volt = 0; // 0.01v / 0 ~ 4.2v

      sendDataHead(ID_Volt);
      write_FrSky16(Datas_Volt); 
   }

   // Altitude
   void send_Altitude(void)
   {
      uint16_t Datas_altitude_bp;
      uint16_t Datas_altitude_ap;
      static uint16_t Start_altitude;

      if (!f.ARMED)
      {
         Start_altitude = alt.EstAlt / 100;
      }

      Datas_altitude_bp = (alt.EstAlt / 100) - Start_altitude;
      Datas_altitude_ap = (alt.EstAlt % 100);

      sendDataHead(ID_Altitude_bp);
      write_FrSky16(Datas_altitude_bp);
      sendDataHead(ID_Altitude_ap);
      write_FrSky16(Datas_altitude_ap);
   }

   // GPS speed
   void send_GPS_speed(void)
   {
      if (f.GPS_FIX && GPS_numSat >= 4) 
      {            
         uint16_t Datas_GPS_speed_bp;
         uint16_t Datas_GPS_speed_ap;
         
         Datas_GPS_speed_bp = GPS_speed * 0.036;
         Datas_GPS_speed_ap = 0;
      
         sendDataHead(ID_GPS_speed_bp);
         write_FrSky16(Datas_GPS_speed_bp);
         sendDataHead(ID_GPS_speed_ap);
         write_FrSky16(Datas_GPS_speed_ap);
      }
   }

   // GPS position
   void send_GPS_position(void)
   {
   
         uint16_t Datas_Longitude_bp;
         uint16_t Datas_Longitude_ap;
         uint16_t Datas_E_W;
         uint16_t Datas_Latitude_bp;
         uint16_t Datas_Latitude_ap;
         uint16_t Datas_N_S;
         Datas_Longitude_bp = abs(GPS_coord[LON]) / 100000;
         Datas_Longitude_ap = abs((GPS_coord[LON])/10)  % 10000;
         Datas_E_W = GPS_coord[LON] < 0 ? 'W' : 'E';
         Datas_Latitude_bp = abs(GPS_coord[LAT]) / 100000;
         Datas_Latitude_ap = abs((GPS_coord[LAT])/10) % 10000;
         Datas_N_S = GPS_coord[LAT] < 0 ? 'S' : 'N';

         sendDataHead(ID_Longitude_bp);
         write_FrSky16(Datas_Longitude_bp);
         sendDataHead(ID_Longitude_ap);
         write_FrSky16(Datas_Longitude_ap);
         sendDataHead(ID_E_W);
         write_FrSky16(Datas_E_W);

         sendDataHead(ID_Latitude_bp);
         write_FrSky16(Datas_Latitude_bp);
         sendDataHead(ID_Latitude_ap);
         write_FrSky16(Datas_Latitude_ap);
         sendDataHead(ID_N_S);
         write_FrSky16(Datas_N_S);
      
   }

   // Course
   void send_Course(void)
   {
      uint16_t Datas_Course_bp;
      uint16_t Datas_Course_ap;

      Datas_Course_bp = att.heading;
      Datas_Course_ap = 0;

      sendDataHead(ID_Course_bp);
      write_FrSky16(Datas_Course_bp);
      sendDataHead(ID_Course_ap);
      write_FrSky16(Datas_Course_ap);
   }

   // Time
   void send_Time(void)
   {
      uint32_t seconds_since_start = millis() / 1000;
            
      uint16_t Datas_Date_month;
      uint16_t Datas_Year;
      uint16_t Datas_Minutes_hours;
      uint16_t Datas_seconds;
            
      Datas_Date_month = 0;
      Datas_Year = 12;
      Datas_Minutes_hours = (seconds_since_start / 60) % 60;
      Datas_seconds = seconds_since_start % 60;      
      
      sendDataHead(ID_Hour_Minute);
      write_FrSky16(Datas_Minutes_hours);
      sendDataHead(ID_Second);
      write_FrSky16(Datas_seconds);
   }

   // ACC
   /*void send_Accel(void)
   {
      int16_t Datas_Acc_X;
      int16_t Datas_Acc_Y;
      int16_t Datas_Acc_Z;

      Datas_Acc_X = ((float)accSmooth[0] / acc_1G) * 1000;
      Datas_Acc_Y = ((float)accSmooth[1] / acc_1G) * 1000;
      Datas_Acc_Z = ((float)accSmooth[2] / acc_1G) * 1000;

      sendDataHead(ID_Acc_X);
      write_FrSky16(Datas_Acc_X);
      sendDataHead(ID_Acc_Y);
      write_FrSky16(Datas_Acc_Y);
      sendDataHead(ID_Acc_Z);
      write_FrSky16(Datas_Acc_Z);      
   }*/

   // Voltage (Ampere Sensor)  
   void send_Voltage_ampere(void)
   {
   
      #if defined (FAS_100)   // todo   !!!!!!!!!!!!!!!!!
      {
         uint16_t Datas_Voltage_Amp_bp;
         uint16_t Datas_Voltage_Amp_ap;
         uint16_t Datas_Current;   

         Datas_Voltage_Amp_bp = 0;
         Datas_Voltage_Amp_ap = 0;   
         Datas_Current = 0;

         sendDataHead(ID_Voltage_Amp_bp);
         write_FrSky16(Datas_Voltage_Amp_bp);
         sendDataHead(ID_Voltage_Amp_ap);
         write_FrSky16(Datas_Voltage_Amp_ap);   
         sendDataHead(ID_Current);
         write_FrSky16(Datas_Current);
      }
      #else   // use vBat
      {
         uint16_t Datas_Voltage_vBat_bp;
         uint16_t Datas_Voltage_vBat_ap;   
         uint16_t voltage;
         voltage = (vbat * 110) / 21;          
         Datas_Voltage_vBat_bp = voltage / 100;
         Datas_Voltage_vBat_ap = ((voltage % 100) + 5) / 10;         

         sendDataHead(ID_Voltage_Amp_bp);
         write_FrSky16(Datas_Voltage_vBat_bp);
         sendDataHead(ID_Voltage_Amp_ap);
         write_FrSky16(Datas_Voltage_vBat_ap);   
      }
      #endif
   }

#endif

on main.ino (e.g) line 620

Code: Select all

#if defined(TELEMETRY_FRSKY) 
   telemetry_frsky();
  #endif

config.h (e.g) line 830

Code: Select all

    
  /********************************************************************/
  /********                   FrSky  telemetry                *********/
  /********************************************************************/
    #define TELEMETRY_FRSKY
    #define KV_MOTOR 1120.0       // kv of motors in rpm/V

It includes already the Quadbow RPM mod

not tested yet because I am with no time at all but it will compile just fine so, you tell me how it went

Fly safe.
Cheers,
KV

fadz · Post by **fadz** » Fri Apr 26, 2013 12:09 pm

Here is the newst sketch comaptible with r1413 and with MultiWiiEzGui

Also flight tested with Turnigy 9xr and FrSky Telemetry Module.

Code: Select all

// ****************************************************************
// FrSky telemetry
// Version: 0.2.2
// Date 7/10/2012
// Changes: V0.2.2: - corrected ID_Course_ap is 0x1C not 0x24 protocol says 0x14+8
// Date 20/09/2012
// Changes: V0.2.1: - make it work with 2.1 (shared dev)
// Date: 14/08/2012
// Changes: V0.2: - Byte stuffing added
//                - vBat will be send, if "#define FAS_100" is comment out
//                V0.1: - First release
// ****************************************************************

#if defined(TELEMETRY_FRSKY)
  // user defines
  //#define FAS_100  //if commment out, MultiWii vBat voltage will be send instead of FrSky FAS 100 voltage


  // Serial config datas
  #define TELEMETRY_FRSKY_SERIAL 3
  #define TELEMETRY_FRSKY_BAUD 9600  

  // Timing
  #define Time_telemetry_send 125000 
  static uint8_t cycleCounter = 0;
  static uint32_t FrSkyTime  = 0;

  // Frame protocol
  #define Protocol_Header   0x5E
  #define Protocol_Tail      0x5E
  
  
  // Data Ids  (bp = before point; af = after point)
  // Official data IDs
  #define ID_GPS_altidute_bp    0x01
  #define ID_GPS_altidute_ap    0x09
  #define ID_Temprature1        0x02
  #define ID_RPM                0x03
  #define ID_Fuel_level         0x04
  #define ID_Temprature2        0x05
  #define ID_Volt               0x06
  #define ID_Altitude_bp        0x10
  #define ID_Altitude_ap        0x21
  #define ID_GPS_speed_bp       0x11
  #define ID_GPS_speed_ap       0x19
  #define ID_Longitude_bp       0x12
  #define ID_Longitude_ap       0x1A
  #define ID_E_W                0x22
  #define ID_Latitude_bp        0x13
  #define ID_Latitude_ap        0x1B
  #define ID_N_S                0x23
  #define ID_Course_bp          0x14
  #define ID_Course_ap          0x24
  #define ID_Date_Month         0x15
  #define ID_Year               0x16
  #define ID_Hour_Minute        0x17
  #define ID_Second             0x18
  #define ID_Acc_X              0x24
  #define ID_Acc_Y              0x25
  #define ID_Acc_Z              0x26
  #define ID_Voltage_Amp_bp     0x3A
  #define ID_Voltage_Amp_ap     0x3B
  #define ID_Current            0x28
  // User defined data IDs
  #define ID_Gyro_X             0x40
  #define ID_Gyro_Y             0x41
  #define ID_Gyro_Z             0x42

  //Multiwii EZ-GUI
  #define ID_Ang_X             0x50
  #define ID_Ang_Y             0x51
  #define ID_Ang_Z             0x52 

   // Main function FrSky telemetry
    void telemetry_frsky() 
   {         
      if (currentTime > FrSkyTime ) // 
      {          
         FrSkyTime = currentTime + Time_telemetry_send;
         cycleCounter++;
         // Datas sent every 125 ms
            send_Accel();
            sendAngles();
            sendDataTail();   

         if ((cycleCounter % 4) == 0) 
         {      
            // Datas sent every 500ms
            send_Altitude();
            send_RPM();
            send_Course();
            send_GPS_speed();
            //send_Cell_volt();       todo
            sendDataTail();   

         }
         if ((cycleCounter % 8) == 0) 
         {      
            // Datas sent every 1s            
            //send_Time();
            send_GPS_position();
            send_GPS_altitude();
            send_Temperature2();  // Distance_to_home
            //send_Fuel_level();
            send_Voltage_ampere();
            send_Temperature1();  // num of Sats
            sendDataTail();            
         }

         if (cycleCounter == 40) 
         {
            // Datas sent every 5s
            cycleCounter = 0;       
         }
      }
   }

   void write_FrSky8(uint8_t Data)
   {
      SerialWrite(TELEMETRY_FRSKY_SERIAL, Data);
   }

   void write_FrSky16(uint16_t Data)
   {
      uint8_t Data_send;
      Data_send = Data;      
      check_FrSky_stuffing(Data_send);
      Data_send = Data >> 8 & 0xff;
      check_FrSky_stuffing(Data_send);
   }
   
   void check_FrSky_stuffing(uint8_t Data) //byte stuffing
   {
      if (Data == 0x5E)   
      {
         write_FrSky8(0x5D);
         write_FrSky8(0x3E);
      }
      else if (Data == 0x5D)   
      {
         write_FrSky8(0x5D);
         write_FrSky8(0x3D);
      }
      else
      {
         write_FrSky8(Data);         
      }
   }

   static void sendDataHead(uint8_t Data_id)
   {
      write_FrSky8(Protocol_Header);
      write_FrSky8(Data_id);
   }

   static void sendDataTail(void)
   {
      write_FrSky8(Protocol_Tail);      
   }


   //*********************************************************************************
   //-----------------   Telemetrie Datas   ------------------------------------------   
   //*********************************************************************************

   // GPS altitude
   void send_GPS_altitude(void)
   {          
      if (f.GPS_FIX && GPS_numSat >= 4) 
      {
         int16_t Datas_GPS_altidute_bp;
         uint16_t Datas_GPS_altidute_ap;

         Datas_GPS_altidute_bp = GPS_altitude;
         Datas_GPS_altidute_ap = 0;

         sendDataHead(ID_GPS_altidute_bp);
         write_FrSky16(Datas_GPS_altidute_bp);
         sendDataHead(ID_GPS_altidute_ap);
         write_FrSky16(Datas_GPS_altidute_ap);
      }
   }
   
   // Temperature
   void send_Temperature1(void)
   {
      int16_t Datas_Temprature1;

      Datas_Temprature1 = GPS_numSat;  // Number of Satalits alias Temp1

      sendDataHead(ID_Temprature1);
      write_FrSky16(Datas_Temprature1); 
   }

   // RPM
   void inline send_RPM(void)
   {
       uint16_t Data_RPM = 0;
       float rpm; 

       for (uint8_t i=0; i<NUMBER_MOTOR; i++)
 {
       Data_RPM += motor[i] - MINCOMMAND;
}
       #if defined(VBAT)
       rpm = ((float) Data_RPM * (float) analog.vbat * KV_MOTOR) / ((float) NUMBER_MOTOR * 300.0 * (float) (MAXTHROTTLE - MINCOMMAND));
       #else
       rpm = ((float) Data_RPM * 120.0 * KV_MOTOR) / ((float) NUMBER_MOTOR * 300.0 * (float) (MAXTHROTTLE - MINCOMMAND));
       #endif
       Data_RPM = (uint16_t) rpm;

       sendDataHead(ID_RPM);
       write_FrSky16(Data_RPM);
}


   // Fuel level
   void send_Fuel_level(void)
   {
      uint16_t Datas_Fuel_level;

      Datas_Fuel_level = 0; 

      sendDataHead(ID_Fuel_level);
      write_FrSky16(Datas_Fuel_level); 
   }

   // Temperature 2
   void send_Temperature2(void)
   {
      if (f.GPS_FIX_HOME)
      {
         int16_t Datas_Temprature2;

         Datas_Temprature2 = GPS_distanceToHome; // Distance to home alias Temp2

         sendDataHead(ID_Temprature2);
         write_FrSky16(Datas_Temprature2);  
      }      
   }

   // Cell voltage  todo !!!!!!!!!!!!!!!!!!
   void send_Cell_volt(void) // Datas FrSky FLVS-01 voltage sensor
   {
      uint16_t Datas_Volt;
      uint8_t number_of_cells = 0;   // LiPo 3S = 3; LiPo 4S = 4 ...
      static uint8_t cell = 0;
      if (cell >= number_of_cells); cell = 0;
      
      Datas_Volt = 0; // 0.01v / 0 ~ 4.2v

      sendDataHead(ID_Volt);
      write_FrSky16(Datas_Volt); 
   }

   // Altitude
   void send_Altitude(void)
   {
      uint16_t Datas_altitude_bp;
      uint16_t Datas_altitude_ap;
      static uint16_t Start_altitude;

      if (!f.ARMED)
      {
         Start_altitude = alt.EstAlt / 100;
      }

      Datas_altitude_bp = (alt.EstAlt / 100) - Start_altitude;
      Datas_altitude_ap = (alt.EstAlt % 100);

      sendDataHead(ID_Altitude_bp);
      write_FrSky16(Datas_altitude_bp);
      sendDataHead(ID_Altitude_ap);
      write_FrSky16(Datas_altitude_ap);
   }

   // GPS speed
   void send_GPS_speed(void)
   {
      if (f.GPS_FIX && GPS_numSat >= 4) 
      {            
         uint16_t Datas_GPS_speed_bp;
         uint16_t Datas_GPS_speed_ap;
         
         Datas_GPS_speed_bp = GPS_speed * 0.036;
         Datas_GPS_speed_ap = 0;
      
         sendDataHead(ID_GPS_speed_bp);
         write_FrSky16(Datas_GPS_speed_bp);
         sendDataHead(ID_GPS_speed_ap);
         write_FrSky16(Datas_GPS_speed_ap);
      }
   }

   // GPS position
   void send_GPS_position(void)
   {
   
         uint16_t Datas_Longitude_bp;
         uint16_t Datas_Longitude_ap;
         uint16_t Datas_E_W;
         uint16_t Datas_Latitude_bp;
         uint16_t Datas_Latitude_ap;
         uint16_t Datas_N_S;
         Datas_Longitude_bp = abs(GPS_coord[LON]) / 100000;
         Datas_Longitude_ap = abs((GPS_coord[LON])/10)  % 10000;
         Datas_E_W = GPS_coord[LON] < 0 ? 'W' : 'E';
         Datas_Latitude_bp = abs(GPS_coord[LAT]) / 100000;
         Datas_Latitude_ap = abs((GPS_coord[LAT])/10) % 10000;
         Datas_N_S = GPS_coord[LAT] < 0 ? 'S' : 'N';

         sendDataHead(ID_Longitude_bp);
         write_FrSky16(Datas_Longitude_bp);
         sendDataHead(ID_Longitude_ap);
         write_FrSky16(Datas_Longitude_ap);
         sendDataHead(ID_E_W);
         write_FrSky16(Datas_E_W);

         sendDataHead(ID_Latitude_bp);
         write_FrSky16(Datas_Latitude_bp);
         sendDataHead(ID_Latitude_ap);
         write_FrSky16(Datas_Latitude_ap);
         sendDataHead(ID_N_S);
         write_FrSky16(Datas_N_S);
      
   }

   // Course
   void send_Course(void)
   {
      uint16_t Datas_Course_bp;
      uint16_t Datas_Course_ap;

      Datas_Course_bp = att.heading;
      Datas_Course_ap = 0;

      sendDataHead(ID_Course_bp);
      write_FrSky16(Datas_Course_bp);
      sendDataHead(ID_Course_ap);
      write_FrSky16(Datas_Course_ap);
   }

   // Time
   void send_Time(void)
   {
      uint32_t seconds_since_start = millis() / 1000;
            
      uint16_t Datas_Date_month;
      uint16_t Datas_Year;
      uint16_t Datas_Minutes_hours;
      uint16_t Datas_seconds;
            
      Datas_Date_month = 0;
      Datas_Year = 12;
      Datas_Minutes_hours = (seconds_since_start / 60) % 60;
      Datas_seconds = seconds_since_start % 60;      
      
      sendDataHead(ID_Hour_Minute);
      write_FrSky16(Datas_Minutes_hours);
      sendDataHead(ID_Second);
      write_FrSky16(Datas_seconds);
   }

   // ACC
   void send_Accel(void)
   {
      int16_t Datas_Acc_X;
      int16_t Datas_Acc_Y;
      int16_t Datas_Acc_Z;

      Datas_Acc_X = ((float)imu.accSmooth[0] / ACC_1G) * 1000;
      Datas_Acc_Y = ((float)imu.accSmooth[1] / ACC_1G) * 1000;
      Datas_Acc_Z = ((float)imu.accSmooth[2] / ACC_1G) * 1000;

      sendDataHead(ID_Acc_X);
      write_FrSky16(Datas_Acc_X);
      sendDataHead(ID_Acc_Y);
      write_FrSky16(Datas_Acc_Y);
      sendDataHead(ID_Acc_Z);
      write_FrSky16(Datas_Acc_Z);      
   }

   // angles EZ-GUI
   void sendAngles(void)
   {
       int16_t Datas_Ang_X;
       int16_t Datas_Ang_Y;
       int16_t Datas_Acc_Z;

       Datas_Ang_X = angle[0];//((float)accSmooth[0] / acc_1G) * 1000;
       Datas_Ang_Y = angle[1];//((float)accSmooth[1] / acc_1G) * 1000;
       Datas_Acc_Z = angle[2];//((float)accSmooth[2] / acc_1G) * 1000;

       sendDataHead(ID_Ang_X);
       write_FrSky16(Datas_Ang_X);
       sendDataHead(ID_Ang_Y);
       write_FrSky16(Datas_Ang_Y);
       sendDataHead(ID_Acc_Z);
       write_FrSky16(Datas_Acc_Z);      
   }

   // Voltage (Ampere Sensor)  
   void send_Voltage_ampere(void)
   {
   
      #if defined (FAS_100)   // todo   !!!!!!!!!!!!!!!!!
      {
         uint16_t Datas_Voltage_Amp_bp;
         uint16_t Datas_Voltage_Amp_ap;
         uint16_t Datas_Current;   

         Datas_Voltage_Amp_bp = 0;
         Datas_Voltage_Amp_ap = 0;   
         Datas_Current = 0;

         sendDataHead(ID_Voltage_Amp_bp);
         write_FrSky16(Datas_Voltage_Amp_bp);
         sendDataHead(ID_Voltage_Amp_ap);
         write_FrSky16(Datas_Voltage_Amp_ap);   
         sendDataHead(ID_Current);
         write_FrSky16(Datas_Current);
      }
      #else   // use vBat
      {
         uint16_t Datas_Voltage_vBat_bp;
         uint16_t Datas_Voltage_vBat_ap;   
         uint16_t voltage;
         voltage = (analog.vbat * 110) / 21;          
         Datas_Voltage_vBat_bp = voltage / 100;
         Datas_Voltage_vBat_ap = ((voltage % 100) + 5) / 10;         

         sendDataHead(ID_Voltage_Amp_bp);
         write_FrSky16(Datas_Voltage_vBat_bp);
         sendDataHead(ID_Voltage_Amp_ap);
         write_FrSky16(Datas_Voltage_vBat_ap);   
      }
      #endif
   }

#endif