Naza-Canbus -> Frsky s-port

Si vous avez suivit le sujet Zaggotelemetry

Le principe est presque le même sauf que la on va décoder le CAN-BUS du Naza

L'avantage est de récupérer beaucoup plus d'information ...
pour n'en citer que quelque un :
Pitch, Roll, Mode de vol, tension Lipo, latitude, longitude, Gps altitude, altitude baro, cog, vitess, vsi,fix gps, nb sat, cap ... 


Le schéma est assez simple 

CAN-BUS NAZA -> conversion des signaux can -> serie -> Microcontroleur Teensy 3.1 -> S-port Frsky

Voilà a quoi ça ressemble une fois fini :




Coté matériel il faut :

1 Teensy 3.1 moins de 20 Euros 



1 Convertiseur CAN / serie moins de 1 Euro




Il faudra le raccourcir un peut pour obtenir quelque chose d'encore plus compact 








quelques morceaux de fil et connecteur que vous avez tous dans vos tiroir



Coté code j'ai réunis et modifier des morceaux existant sur le Rcgroup 

une partie du nazadécodeur de Pawelsky http://www.rcgroups.com/forums/showthread.php?t=1995704

et une partie du code mavlink -> frsky  faite par Rolf Blomgren http://diydrones.com/forum/topics/amp-to-frsky-x8r-sport-converter?id=705844:Topic:1539556&page=1#comments


La partie de code que j'ai écrit et modifié est ici :

http://f8byc1.free.fr/taranis/naza-frsky/Naza-FrSky.zip



Naza-FrSky.ino et FrSkySPort.ino

Code:

/*
  DJI Naza (v1/V2 + PMU) CAN data decoder library example
  Outputs data to a serial port
  (c) Pawelsky 20140921
  Not for commercial use

  Requires FlexCan library v0.1-beta (or newer if compatible)
  https://github.com/teachop/FlexCAN_Library/releases/tag/v0.1-beta

  Requires NazaCanDecoder library by Pawelsky
  http://www.rcgroups.com/forums/showpost.php?p=27129264&postcount=1

  Requires Teensy 3.1 board and CAN transceiver
  Complie with "CPU speed" set to "96MHz (overclock)"
  Refer to naza_can_decoder_wiring.jpg diagram for proper connection
  Connections can be greatly simplified using CAN bus and MicroSD shields by Pawelsky
  (see teensy_shields.jpg for installation and naza_can_decoder_wiring_shields.jpg for wiring)
*/

#include "NazaCanDecoderLib.h"
#include "FlexCAN.h"
#include "FrSkySPort.h"

// Use Serial to output data via USB or Serial1 to use Teensy's first serial port
#define SERIAL_PORT Serial

uint32_t currTime, attiTime, otherTime, clockTime;
char dateTime[20];
uint32_t messageId;

int led = 13;




void setup()
{
  pinMode(led, OUTPUT);
  SERIAL_PORT.begin(115200);
  FrSkySPort_Init();
  NazaCanDecoder.begin();
}

void loop()
{
  messageId = NazaCanDecoder.decode();
//  if(messageId) { SERIAL_PORT.print("Message "); SERIAL_PORT.print(messageId, HEX); SERIAL_PORT.println(" decoded"); }

  currTime = millis();

  // Display attitude at 10Hz rate so every 100 milliseconds
  if(attiTime < currTime)
  {
    attiTime = currTime + 100;
    SERIAL_PORT.print("Pitch: "); SERIAL_PORT.print(NazaCanDecoder.getPitch());
    SERIAL_PORT.print(", Roll: "); SERIAL_PORT.println(NazaCanDecoder.getRoll());
  }

  // Display other data at 5Hz rate so every 200 milliseconds
  if(otherTime < currTime)
  {
    otherTime = currTime + 200;
    SERIAL_PORT.print("Mode: ");
    switch (NazaCanDecoder.getMode())
    {
      case NazaCanDecoderLib::MANUAL:   SERIAL_PORT.print("MAN"); break;
      case NazaCanDecoderLib::GPS:      SERIAL_PORT.print("GPS"); break;
      case NazaCanDecoderLib::FAILSAFE: SERIAL_PORT.print("FS");  break;
      case NazaCanDecoderLib::ATTI:     SERIAL_PORT.print("ATT"); break;
      default:                          SERIAL_PORT.print("UNK");
    }
    SERIAL_PORT.print(", Bat: "); SERIAL_PORT.println(NazaCanDecoder.getBattery() / 1000.0, 2);

    SERIAL_PORT.print("Lat: "); SERIAL_PORT.print(NazaCanDecoder.getLat(), 7);
    SERIAL_PORT.print(", Lon: "); SERIAL_PORT.print(NazaCanDecoder.getLon(), 7);
    SERIAL_PORT.print(", GPS alt: "); SERIAL_PORT.print(NazaCanDecoder.getGpsAlt());
    SERIAL_PORT.print(", COG: "); SERIAL_PORT.print(NazaCanDecoder.getCog());
    SERIAL_PORT.print(", Speed: "); SERIAL_PORT.print(NazaCanDecoder.getSpeed());
    SERIAL_PORT.print(", VSI: "); SERIAL_PORT.print(NazaCanDecoder.getVsi());
    SERIAL_PORT.print(", Fix: ");
    switch (NazaCanDecoder.getFixType())
    {
      case NazaCanDecoderLib::NO_FIX:   SERIAL_PORT.print("No fix"); break;
      case NazaCanDecoderLib::FIX_2D:   SERIAL_PORT.print("2D");     break;
      case NazaCanDecoderLib::FIX_3D:   SERIAL_PORT.print("3D");     break;
      case NazaCanDecoderLib::FIX_DGPS: SERIAL_PORT.print("DGPS");   break;
      default:                          SERIAL_PORT.print("UNK");
    }
    SERIAL_PORT.print(", Sat: "); SERIAL_PORT.println(NazaCanDecoder.getNumSat());

    SERIAL_PORT.print("Alt: "); SERIAL_PORT.print(NazaCanDecoder.getAlt());
    SERIAL_PORT.print(", Heading: "); SERIAL_PORT.println(NazaCanDecoder.getHeading());
  }

  // Display date/time at 1Hz rate so every 1000 milliseconds
  if(clockTime < currTime)
  {
    
    
    clockTime = currTime + 1000;
    sprintf(dateTime, "%4u.%02u.%02u %02u:%02u:%02u",
            NazaCanDecoder.getYear() + 2000, NazaCanDecoder.getMonth(), NazaCanDecoder.getDay(),
            NazaCanDecoder.getHour(), NazaCanDecoder.getMinute(), NazaCanDecoder.getSecond());
    SERIAL_PORT.print("Date/Time: "); SERIAL_PORT.println(dateTime);
    
    

    
    
  }

  NazaCanDecoder.heartbeat();
  
      FrSkySPort_Process();
  
}

Code:

#include "FrSkySPort.h"

#define _FrSkySPort_Serial            Serial1
#define _FrSkySPort_C1                UART0_C1
#define _FrSkySPort_C3                UART0_C3
#define _FrSkySPort_S2                UART0_S2
#define _FrSkySPort_BAUD           57600
#define   MAX_ID_COUNT              19

short crc;                         // used for crc calc of frsky-packet
uint8_t lastRx;
uint32_t FR_ID_count = 0;
uint8_t cell_count = 0;
uint8_t latlong_flag = 0;
uint32_t latlong = 0;
uint8_t first=0;

// POUR TEST
int32_t    ap_latitude = 0;              // 585522540;
int32_t    ap_longitude = 0;            // 162344467;
int32_t    ap_gps_altitude = 0;        // 1000 = 1m


// ***********************************************************************
void FrSkySPort_Init(void)  {
      _FrSkySPort_Serial.begin(_FrSkySPort_BAUD);
      _FrSkySPort_C3 = 0x10;            // Tx invert
      _FrSkySPort_C1= 0xA0;            // Single wire mode
      _FrSkySPort_S2 = 0x10;           // Rx Invert
      
}

// ***********************************************************************
void FrSkySPort_Process(void) {
   uint8_t data = 0;
        uint32_t temp=0;
   uint8_t offset;
        while ( _FrSkySPort_Serial.available())
          {
     data =  _FrSkySPort_Serial.read();
          if (lastRx == START_STOP && ((data == SENSOR_ID1) || (data == SENSOR_ID2) || (data == SENSOR_ID3)  || (data == SENSOR_ID4)))
            {
            
              switch(FR_ID_count) {
                 case 0:
                   FrSkySPort_SendPackage(FR_ID_ALTITUDE,NazaCanDecoder.getAlt() * 100 );  //
                 break;
                 case 1:
                
switch (NazaCanDecoder.getFixType())
    {
      case NazaCanDecoderLib::NO_FIX:   FrSkySPort_SendPackage(FR_ID_T2,0 ); break;
      case NazaCanDecoderLib::FIX_2D:   FrSkySPort_SendPackage(FR_ID_T2,2 ); break;
      case NazaCanDecoderLib::FIX_3D:   FrSkySPort_SendPackage(FR_ID_T2,3 ); break;
      case NazaCanDecoderLib::FIX_DGPS: FrSkySPort_SendPackage(FR_ID_T2,4 ); break;
      default:                          FrSkySPort_SendPackage(FR_ID_T2,99 );
    }
                 break;
    
                 case 2:
                   FrSkySPort_SendPackage(FR_ID_GPS_ALT,NazaCanDecoder.getGpsAlt()*100);          
                 break;  
                
                 case 3:
                   FrSkySPort_SendPackage(FR_ID_VFAS,NazaCanDecoder.getBattery() /10);          
                 break;

                case 4:        // Sends the ap_longitude value, setting bit 31 high
                ap_longitude = abs(NazaCanDecoder.getLon()*10000000);
                  
                       if(ap_longitude < 0)
                           latlong=((abs(ap_longitude)/100)*6)  | 0xC0000000;
                           else
                           latlong=((abs(ap_longitude)/100)*6)  | 0x80000000;
                       FrSkySPort_SendPackage(FR_ID_LATLONG,latlong);
                      
                   break;
                  
                 case 5:        // Sends the ap_latitude value, setting bit 31 low  
                 ap_latitude = abs(NazaCanDecoder.getLat()*10000000);    
                         if(ap_latitude < 0 )
                             latlong=((abs(ap_latitude)/100)*6) | 0x40000000;
                             else
                             latlong=((abs(ap_latitude)/100)*6);
                         FrSkySPort_SendPackage(FR_ID_LATLONG,latlong);
                        
                    break;  
                    
                    
                    
                    
                    
                 case 6:
                   FrSkySPort_SendPackage(FR_ID_HEADING,NazaCanDecoder.getHeading()*100 );          
                 break;                    
                    
                    
    
                      case 7:
switch (NazaCanDecoder.getMode())
    {
      case NazaCanDecoderLib::MANUAL:   FrSkySPort_SendPackage(FR_ID_T1,0 ); break;
      case NazaCanDecoderLib::ATTI:     FrSkySPort_SendPackage(FR_ID_T1,1 ); break;
      case NazaCanDecoderLib::GPS:      FrSkySPort_SendPackage(FR_ID_T1,2 ); break;
      case NazaCanDecoderLib::FAILSAFE: FrSkySPort_SendPackage(FR_ID_T1,3 ); break;

      default:                          FrSkySPort_SendPackage(FR_ID_T2,99 );
    }    
               break;                
              
              
                      case 8:              
              
               FrSkySPort_SendPackage(FR_ID_FUEL,NazaCanDecoder.getNumSat() ); break;
              
              
              
              
              
              
              
              
                
                
               }
            FR_ID_count++;
            if(FR_ID_count > MAX_ID_COUNT) FR_ID_count = 0;  
            }
          lastRx=data;
          }
}


// ***********************************************************************
void FrSkySPort_SendByte(uint8_t byte) {
   
       _FrSkySPort_Serial.write(byte);
   
        // CRC update
   crc += byte;         //0-1FF
   crc += crc >> 8;   //0-100
   crc &= 0x00ff;
   crc += crc >> 8;   //0-0FF
   crc &= 0x00ff;
}


// ***********************************************************************
void FrSkySPort_SendCrc() {
   _FrSkySPort_Serial.write(0xFF-crc);
        crc = 0;          // CRC reset
}


// ***********************************************************************
void FrSkySPort_SendPackage(uint16_t id, uint32_t value) {
            
//   if(MavLink_Connected) {
        if (1==1) {
            
            }
            
        digitalWrite(led,HIGH);    
        _FrSkySPort_C3 |= 32;      //  Transmit direction, to S.Port
   FrSkySPort_SendByte(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();
   _FrSkySPort_Serial.flush();
   _FrSkySPort_C3 ^= 32;      // Transmit direction, from S.Port

        digitalWrite(led,LOW);
        
}

FrskySPort.h

Code:

// Frsky Sensor-ID to use.
#define SENSOR_ID1                   0x1B // ID of sensor. Must be something that is polled by FrSky RX
#define SENSOR_ID2                   0x0D
#define SENSOR_ID3                   0x34
#define SENSOR_ID4                   0x67
// Frsky-specific
#define START_STOP               0x7e
#define DATA_FRAME               0x10


//Frsky DATA ID's
#define FR_ID_SPEED               0x0830
#define FR_ID_VFAS                 0x0210
#define FR_ID_CURRENT         0x0200
#define FR_ID_RPM                  0x050F      
#define FR_ID_ALTITUDE         0x0100
#define FR_ID_FUEL                 0x0600  
#define FR_ID_ADC1                0xF102  
#define FR_ID_ADC2                0xF103      
#define FR_ID_LATLONG         0x0800
#define FR_ID_CAP_USED       0x0600
#define FR_ID_VARIO               0x0110
#define FR_ID_CELLS              0x0300    
#define FR_ID_CELLS_LAST   0x030F      
#define FR_ID_HEADING          0x0840
#define FR_ID_ACCX               0x0700
#define FR_ID_ACCY               0x0710
#define FR_ID_ACCZ               0x0720
#define FR_ID_T1                     0x0400
#define FR_ID_T2                     0x0410
#define FR_ID_GPS_ALT          0x0820



Il vous faut utiliser les libraries de ces 2 liens :

la partie Frsky est ici : MavLink_FrSkySPort_1.3.zip

la partie décodeur can est la  NazaDecoder20141130 NazaDecoder library for Arduino v20141130 (86.8 KB) 276 views zip






ATTENTION cela fonctionne en l'état mais tout n'est pas fini de tester


A cela il faudra rajouter de beau scripts LUA (en cours de modification car c'est pas le même combat que la Dropix   )


Bonne lecture 


Sylvain

Super sympa !
ça me tente bien d'essayer ça.

Pour le teensy 3.1 il faut plutôt compter 30€ avec les frais de port.


j'attends de voir la suite !

Salut

Très intéressant

J'attends la suite pour appliquer sur mon QAV400

J'ai ajouter le schéma au tuto !

chez moi sur la table c'est déjà fonctionnel 

il faut que je refasse bien le tour de toutes les données possible (j'ai pas encore tout exploité dans le prog mais les données les plus importantes sont la ! )


Dans la 1ere partie du code il y en a les 3/4 qui ne servent a rien ça revoie juste les données récupérés sur le Naza et ça les renvoie sur le port série (USB)

il suffit d'ouvrir un terminal pour voir ce que l'on reçoit !


Pour les prix j'ai commander chez SNOOTLAB http://snootlab.com/pjrc-teensy/517-teensy-31-usb-fr.html





J'ai demandé a Banggood qu'il en stock on verra bien ...

Sylvain,
Quel est la taille du scketch ?
Ça passe avec une Arduino NANO ? Car les NANO sont moins couteuse qu'une TEENYS (cf. voir banggood).
Si tu peux faire un test ... et me dire ...
A+Bruno

Arffff c'est un problème de hard après pour faire du Sport sur arduino tu ne peut pas sans inversé les signaux ... que la pas de composants en plus !

Pour ce qui est des QAV400 il suffit qu'il ait un NAZA avec un Can bus donc V2 ou
V1 + PMU ou Lite craqué V2 + PMU

Pour ce qui est de la suite le script a modifier est la :

https://frskytaranis.forumactif.org/t1746-annonces-vocal-pour-dropix-pixhawk-amp#17899

il faut simplement adapter un peut

et pour les données qui ne sont pas encore la 

il faut poursuivre avec de qu'il manque après :

case 8:   
 FrSkySPort_SendPackage(FR_ID_FUEL,NazaCanDecoder.getNumSat() ); break;


et ajouter ce qu'il manque




courage courage il faut participer ...


le plus dur est déjà fait !

djsyl a écrit:Arffff c'est un problème de hard après pour faire du Sport sur arduino tu ne peut pas sans inversé les signaux ... que la pas de composants en plus !
...

Euh, c'est pas si compliqué, je les fais pour l'ArduIMU, jette un coup d'oeil là-dessus : https://frskytaranis.forumactif.org/t1291-arduimu-v3-gps-s-port

Ca vaut juste 80$ un ardu imu V3

Je dit pas que c'est infaisable sur un arduino mini mais que ça ajoute du hard pour l'inversion sport 

en plus pour le can il faut aller quand même assez vite ...

au faite il y a des 10 dof a 10€ chez banggood avec un arduino mini a 3 € ça fait une carte de vol full multiwii a pas cher

djsyl a écrit:Ca vaut juste 80$ un ardu imu V3

Je dit pas que c'est infaisable un un arduino mini mais que ça ajoute du hard pour l'inversion sport 

en plus pour le can il faut aller quand même assez vite ...

Pas besoin d'ajouter du hard, on y arrive très bien avec le soft adhoc.

Je suis en train de porter mon truc sur un Arduino Pro Mini (4$), un GY-87 10DOF (13$) et un GPS Ublox (27$).
La partie GY-87 et S.Port fonctionnent sans soucis et sans hard additionnel.

Par contre, je butte sur la partie GPS Ublox, j'aurais dû rester sur du NMEA tout simple.

J'ai jeté un coup d'oeil sur ton code et il manque quelques éléments dans les fonctions sendByte et sendCrc.

FrSkySPort.ino

Code:


    // ***********************************************************************
    void FrSkySPort_SendByte(uint8_t byte) {
      
//           _FrSkySPort_Serial.write(byte);
       if (( byte == START_STOP ) || ( byte == BYTESTUFF )) {
          _FrSkySPort_Serial.write(BYTESTUFF) ;
          _FrSkySPort_Serial.write(byte ^ STUFF_MASK) ;
       } else {
          _FrSkySPort_Serial.write(byte) ;
       }
      
            // CRC update
       crc += byte;         //0-1FE -> commentaire adéquat
       crc += crc >> 8;   //0-1FF -> commentaire adéquat
       crc &= 0x00ff;      // 0-0FF -> commentaire adéquat
       // crc += crc >> 8;   //0-0FF -> ligne inutile
       // crc &= 0x00ff;      // -> ligne inutile

    }


    // ***********************************************************************
    void FrSkySPort_SendCrc() {
      //  _FrSkySPort_Serial.write(0xFF-crc);
      crc = 0xFF-crc
      if (( crc  == START_STOP ) || ( crc  == BYTESTUFF )) {
         _FrSkySPort_Serial.write(BYTESTUFF) ;
         _FrSkySPort_Serial.write(crc  ^ STUFF_MASK) ;
      }else {
         _FrSkySPort_Serial.write(crc ) ;
      }

      crc = 0;          // CRC reset
    }


Et dans FrSkySPort.h, il faut ajouter quelque part :

Code:


#define BYTESTUFF          0x7d
#define STUFF_MASK       0x20


Merci

Je vais regarder ça de prés mais c'est une partie que je n'ai pas modifié !

ça n'a pas l'air de pénaliser le fonctionnement ( je reçoit bien les infos sur le sport du récepteur)

Mauvais calcul des checksum des trames a ton avis ?

si ça fonctionne il doit pas y avoir beaucoup de contrôle au bout

// ***********************************************************************
   void FrSkySPort_SendCrc() {
     //  _FrSkySPort_Serial.write(0xFF-crc);
     crc = 0xFF-crc manque le ";"


je viens de corrigé et recompiler avec tes modifs ... ça fait pas de mal et tout marche encore

Pardon pour le ";", j'ai fait ça à l'arrache dans notepad.

Le calcul du crc donne exctement le même résultat. Les deux dernières lignes ne font rien en fait. Ce code inutile vient de frsky qui a dû le copier de je ne sais où. J'ai vu ça sur github et en lisant attentivement le code, effectivemet, ces deux dernière ligne ne font rien de plus.

Le truc avec le BYTESTUFF, c'est pour éviter d'avoir le START_STOP  (0x7e) qui circule dans les trames. Dans ce cas, le byte est remplacé par deux bytes qui se suivent 0x7D 0x5E. Naturellement, si le BYTESTUFF (0x7D) est dans la trame, il faut aussi le remplacer par deux bytes qui se suivent, 0x7D 0x5D.

A noter que si cela n'a pas été corrigé, openXsensor ne fait pas le bytestuffing du crc ce qui est une erreur.

Si le bytestuffing n'est pas fait, des trames peuvent être perdues sans que l'on s'en apperçoive si la donnée est transférée fréquemment.

Super boulot Djsyl et Sacre

J'avoue être un peut perdu, mais ça m'intéresse toujours.

Niveau conso batterie, peut on avoir le pourcentage restant ou quelque chose du genre mais autre que le voltage ?


cela pourrait intéresser un ami qui débute avec un quad, et il avait trouvé sympa que sur le phantom il y ai l'autonomie restante en pourcentage.

Vue qu'il n'y a pas d'informations sur l'intensité consommé cela ne peut être qu'une projection par rapport a la tension Lipo !

Rien n'interdit d'ajouter un capteur de courant ...

De mon coté j'ajoute toujours le module tension qui donne l'info par éléments je trouve ça beaucoup plus sécure pour les lipo !

Oui moi aussi j'utilise un capteur, mais pour un novice c'est pas toujours simple de savoir ou il en est rendu niveau autonomie quand il entend par exemple 3.9v, 6etc....

Voilà un excellent exercice de LUA

convertir des valeur de 4.2 volts a 3.5 volts en nombre de 100 a 0 %

puis annoncer les valeurs a chaque chngement de 1%

salut,

Super boulot...ça donne envie de faire chauffer le fer a souder...

Va y chauffe

Bonsoir
Super boulot super intéressant je vais essayer de m'en faire un par contre je n'y connait rien en électronique ou peut on trouver le convertisseur can/ série merci d avance pour votre aide.
A bientot

ici il y en a un peut partout a toi de choisir le meilleur prix ( il y a a boire et a manger ) ...

http://www.aliexpress.com/premium/can-transceiver-board.html?ltype=wholesale&SearchText=can+transceiver+board&isPremium=y&d=y&origin=y&initiative_id=SB_20150313223005&isViewCP=y&catId=0

merci djsyl

Le module en situation

Moi j'attends mon cable DJI CAN-BUS DJI CAN-BUS et GO