Direct Frsky telemtry data from MW FC

Direct Frsky telemtry data from MW FC

Postby Lapino » Tue Jun 26, 2012 3:20 pm

Hi,

I was asking myself if there's an implementation for direct data output from the Multiwii-FC to the digital side port of Frsky telemetry receivers.
This way no additional sensors must be used to display height, position or whatever on your Tx.

Frame Protocol of FrSky Telemetry Hub System:
http://www.frsky-rc.com/uploadfile/2011 ... 301692.pdf

Thanks in advance :)
Lapino
 
Posts: 84
Joined: Tue Aug 16, 2011 10:01 am

Re: Direct Frsky telemtry data from MW FC

Postby fiendie » Tue Jun 26, 2012 3:48 pm

Lapino wrote:Hi,

I was asking myself if there's an implementation for direct data output from the Multiwii-FC to the digital side port of Frsky telemetry receivers.
This way no additional sensors must be used to display height, position or whatever on your Tx.

Frame Protocol of FrSky Telemetry Hub System:
http://www.frsky-rc.com/uploadfile/2011 ... 301692.pdf

Thanks in advance :)

Well, not yet anyway ;)

But I like the idea. Instead of using the Sensor Hub you could frame the sensor data with MultiWii and send it to the FrSky Rx directly.
You'd have to convert the signal to RS232 levels, however, and it would probably eat up quite some flash memory.
fiendie
 
Posts: 124
Joined: Fri Apr 20, 2012 4:22 pm

Re: Direct Frsky telemtry data from MW FC

Postby Lapino » Tue Jun 26, 2012 4:18 pm

I've a Flyduino Mega (2560) so flash isn't a problem...free connections either :)
Maybe some of the great coders could help?
I'm relative new to arduino and interrupt handling(if needed).
Lapino
 
Posts: 84
Joined: Tue Aug 16, 2011 10:01 am

Re: Direct Frsky telemtry data from MW FC

Postby arne » Wed Jun 27, 2012 7:48 pm

I've played with this idea some time ago and did some testing. It's possible and it's not too hard, but the following code fragments are just for testing propose and not cleany coded nor complete. You have been warned ;).

These code segments are meant for MW 2.0. To test this you need a Mega, a level converter and a telemetry capable FrSky reciever/transmitter combination. I've used a small lever converter from Ebay and a flashed 9x to display the data, but i'm not certain that the FrSky implementation from the er9x branch is complete and error-free.

P6272622.JPG


The FrSky hub sends 3 different packets:
Frame 1 is send each 200ms. Basically some voltage, temperature and altitude information.
Frame 2 is send each second. I think the most interesting stuff: GPS, Speed, Fuel.
Frame 3 is send each 5s and only consists date and time information. Not very useful.

The example code only transmits the current GPS position. The rest is static demo data from the FrSky Frame Protocal datasheet.

config.h - somewhere around line 347
Code: Select all
/* Frsky Telemetry Output */
#define TELEMETRY_FRSKY
#define TELEMETRY_FRSKY_SERIAL 3


MultiWii_2_0.ino - line 113
Code: Select all
#if defined(TELEMETRY_FRSKY)
  static uint32_t telemetryFrskyTimeFrame1 = 0, telemetryFrskyTimeFrame2 = 0, telemetryFrskyTimeFrame3 = 0;     
#endif


MultiWii_2_0.ino - line 350 - at the end of annexCode() function
Code: Select all
  #if defined(TELEMETRY_FRSKY)
  telemetry_frsky();
  #endif


LCD.ino - at the end
Code: Select all
#if defined(TELEMETRY_FRSKY)
void send_frsky_frame(byte* data, int len )
{
   uint8_t bytestuff = 0;
   for (uint8_t i=0;i<len;i++)
   {
     if ( ( data[i] == 0x5E || data[i] == 0x5e ) && bytestuff > 0 && bytestuff < 4 )
     {
       SerialWrite(TELEMETRY_FRSKY_SERIAL, 0x5D );
       SerialWrite(TELEMETRY_FRSKY_SERIAL, 0x3E );
       bytestuff++;
     }
     else if ( data[i] == 0x5D || data[i] == 0x5d )
     {
       SerialWrite(TELEMETRY_FRSKY_SERIAL, 0x5D );
       SerialWrite(TELEMETRY_FRSKY_SERIAL, 0x3D );
       bytestuff++;
     }
     else
     {
       SerialWrite(TELEMETRY_FRSKY_SERIAL, data[i] );
       if ( data[i] == 0x5E || data[i] == 0x5e )
       {
         bytestuff=1;
       }
       else
       {
         bytestuff++;
       }
     }
   }
}
// convert dd.ddddd format into ddmm
uint16_t GPS_degrees_to_ddmm(uint32_t i) {
  return (int)( ( ( ( i - ( (int)( i / 100000 ) * 100000 ) ) * 0.6 ) + ( (int)( i / 100000 ) * 100000 ) ) / 1000 );   
}

// convert dd.ddddd format into mmmm   
uint16_t GPS_degrees_to_mmmm(uint32_t i) {
  uint32_t n = ( i - ( (int)( i / 100000 ) * 100000 ) ) * 6;   
  return ( n - ( (int)( n / 10000 ) * 10000 ) );
}

void telemetry_frsky() { 
  // Frame 1: Three-axis Acceleration Values, Altitude (variometer-0.01m), Temperature1, Temperature2, Voltage, Current & Voltage (Ampere Sensor) , RPM
  if (currentTime > telemetryFrskyTimeFrame1 +  200000 ) { // 200ms
      uint16_t rpm = 0;
      for (uint8_t i=0;i<NUMBER_MOTOR;i++)
      {
        rpm=rpm+motor[i];
      } 
      rpm=rpm/NUMBER_MOTOR/30;
     
      byte data[49] = {0x5E, 0x24, 0x00, 0x04, // Three-axis Acceleration Values x
                       0x5E, 0x25, 0x80, 0xFF, // Three-axis Acceleration Values y
                       0x5E, 0x26, 0xE0, 0xFE, // Three-axis Acceleration Values z
                       0x5E, 0x10, 0x3C, 0x00, // Altitude
                       0x5E, 0x21, 0x3C, 0x00, // Altitude
                       0x5E, 0x02, 0xEF, 0xFF, // Temperature1
                       0x5E, 0x05, 0xE9, 0xFF, // Temperature2
                       0x5E, 0x06, 0x18, 0x34, // Volt
                       0x5E, 0x28, 0x02, 0x00, // Current & Voltage (Ampere Sensor) Current
                       0x5E, 0x3A, 0x0a, 0x00, // Current & Voltage (Ampere Sensor) Voltage
                       0x5E, 0x3B, 0x05, 0x00, // Current & Voltage (Ampere Sensor) Voltage
                       0x5E, 0x03, rpm, rpm>>8, // RPM
                       0x5E};
      send_frsky_frame(data, 49);
      telemetryFrskyTimeFrame1 = currentTime;
    } 

    // Frame 2: Course, Latitude, Longitude, Speed, Altitude (GPS), Fuel Level
    if ( currentTime > telemetryFrskyTimeFrame2 + 1000000 ) { // 1s
      uint16_t lat_ddmm = GPS_degrees_to_ddmm(GPS_latitude);
      uint16_t lat_mmmm = GPS_degrees_to_mmmm(GPS_latitude);
      uint16_t lon_ddmm = GPS_degrees_to_ddmm(GPS_longitude);
      uint16_t lon_mmmm = GPS_degrees_to_mmmm(GPS_longitude);

      byte data[53] = {0x5E,0x14,0x2c,0x00, // Course
                       0x5E,0x1c,0x03,0x00, // Course
                       0x5E,0x13,lat_ddmm, lat_ddmm>>8, // Latitude
                       0x5E,0x1b,lat_mmmm, lat_mmmm>>8, // Latitude
                       0x5E,0x23,0x4E,0x00, // N/S [4E|53]
                       0x5E,0x12,lon_ddmm, lon_ddmm>>8, // Longitude
                       0x5E,0x1a,lon_mmmm, lon_mmmm>>8, // Longitude
                       0x5E,0x22,0x45,0x00, // E/W [45|57]
                       0x5E,0x11,0x00,0x00, // Speed (GPS)
                       0x5E,0x19,0x93,0x00, // Speed (GPS)
                       0x5E,0x01,0x02,0x00, // Altitude (GPS)
                       0x5E,0x09,0x00,0x00, // Altitude (GPS)
                       0x5E,0x04,0x64,0x00, // Fuel Level
                       0x5E};
        send_frsky_frame(data, 53);
        telemetryFrskyTimeFrame2 = currentTime;
    }     
    // Frame 3: Date, Time
    if (currentTime > telemetryFrskyTimeFrame3 + 5000000 ) { // 5s
      byte data[17] = {0x5E, 0x15, 0x0F, 0x07, // Date/Month
                       0x5E, 0x16, 0x0B, 0x00, // Year
                       0x5E, 0x17, 0x06, 0x12, // Hour/Minute
                       0x5E, 0x18, 0x32, 0x00, // Second
                       0x5E };
      send_frsky_frame(data,17);
      telemetryFrskyTimeFrame3 = currentTime;
   }           
}
#endif //TELEMETRY_FRSKY


I currently have only two STM32 and a 328 copter, hence I've suspended the project at the moment, but maybe someone could use some bits and pieces.
arne
 
Posts: 23
Joined: Sun Feb 19, 2012 8:30 pm

Re: Direct Frsky telemtry data from MW FC

Postby copterrichie » Wed Jun 27, 2012 10:09 pm

Very interesting, question, what is the maximum range of the Frsky telemetry unit? Reason I ask, Bluetooth range is about 100 feet or there about.
copterrichie
 
Posts: 2107
Joined: Sat Feb 19, 2011 8:30 pm

Re: Direct Frsky telemtry data from MW FC

Postby fiendie » Wed Jun 27, 2012 11:56 pm

copterrichie wrote:Very interesting, question, what is the maximum range of the Frsky telemetry unit? Reason I ask, Bluetooth range is about 100 feet or there about.

Same range as the receiver (i.e. 1.5 km).
100 feet sounds a bit optimistic for Bluetooth.
fiendie
 
Posts: 124
Joined: Fri Apr 20, 2012 4:22 pm

Re: Direct Frsky telemtry data from MW FC

Postby Lapino » Thu Jun 28, 2012 10:07 pm

just ordered the required TTL level converter...will test it as soon as it arrives ;)
Lapino
 
Posts: 84
Joined: Tue Aug 16, 2011 10:01 am

Re: Direct Frsky telemtry data from MW FC

Postby copterrichie » Thu Jun 28, 2012 10:15 pm

This would be really nice to get working, we could dump the On-board OSD and handle all of the telemetry on the ground.
copterrichie
 
Posts: 2107
Joined: Sat Feb 19, 2011 8:30 pm

Re: Direct Frsky telemtry data from MW FC

Postby Alexinparis » Thu Jun 28, 2012 10:44 pm

My dream would be a uplink telemetry based on frsky without impacting the 8 first RC channels.
Alexinparis
 
Posts: 1546
Joined: Wed Jan 19, 2011 9:07 pm

Re: Direct Frsky telemtry data from MW FC

Postby arne » Fri Jun 29, 2012 6:29 am

The Frsky transmitter and the receiver have both a RX/TX port combo, but does it actually have an separate up-link channel?
arne
 
Posts: 23
Joined: Sun Feb 19, 2012 8:30 pm

Next

Return to Ideas

Who is online

Users browsing this forum: No registered users and 2 guests