Komunikacja Matek F405 z Arduino po UART

FC oparte na Arduino

Moderatorzy: moderatorzy2014, moderatorzy

IceMAN92q
Posty: 13
Rejestracja: niedziela 14 lut 2016, 17:52
Lokalizacja: Dobra

Komunikacja Matek F405 z Arduino po UART

Post autor: IceMAN92q »

Witam :-) Próbuję wykonać komunikacje pomiędzy Matek F405-CTR, a Arduino. O ile odbieranie danych przebiega bez najmniejszych problemów to wysłanie jakiś informacji, np. danych radia mi nie działa. Poniżej dodaję kod

Kod: Zaznacz cały

#include <SD.h>
#include "protocol.h"
#define INBUF_SIZE 64
#define MSP_SET_RAW_RC        200
#define MSP_SET_RAW_RC_LENGTH 16

static uint8_t inBuf[INBUF_SIZE];
static uint8_t checksum;
static uint8_t commandMW;
static uint8_t offset;
static uint8_t dataSize;
static Protocol p;

int8_t c,n;  
double i = 0;
double angX, angY, angH, angH2;
double roll1, pitch1, yaw1, throttle1;
double M1, M2, M3, M4;

void setup()  
{
  Serial2.begin(115200);
  //logger.init();

  Serial.begin(115200);
  pinMode(13, OUTPUT);
}

void loop() // run over and over
{
  uint8_t  datad = 0;
  uint8_t  *data = & datad;
  //Serial.print(" Witam ");
  // If you dont need a certain log just comment out the two lines for sending and reading
  p.send_msp( MSP_ATTITUDE  ,data, 0);
  readData();
   
  p.send_msp(MSP_MOTOR ,data, 0);
  readData();

  p.send_msp( MSP_RC  ,data, 0);
  readData();

  uint8_t rc_bytes[] = {1600,1500,1500,1500,1500,1500,1500,1500}; 
  send_msp( MSP_SET_RAW_RC , rc_bytes, MSP_SET_RAW_RC_LENGTH); 
   
  Serial.print("Kat x: ");
  Serial.print(angX);
  Serial.print("Kat y: ");
  Serial.print(angY);
  Serial.print("Kat z: ");
  Serial.print(angH);
  Serial.print("\n");
  
  delayMicroseconds(10000); // 1000 Hz
}

void readData() {

  delay(1);

  while (Serial2.available()) {

    byte c = Serial2.read();
    //Serial.println(c);
    if (c_state == IDLE) {
      c_state = (c=='$') ? HEADER_START : IDLE;
    } 
    else if (c_state == HEADER_START) {
      c_state = (c=='M') ? HEADER_M : IDLE;
    } 
    else if (c_state == HEADER_M) {
      c_state = (c=='>') ? HEADER_ARROW : IDLE;
    } 
    else if (c_state == HEADER_ARROW) {

      if (c > INBUF_SIZE) {  // now we are expecting the payload size
        c_state = IDLE; } 
      else {
        dataSize = c;
        offset = 0;
        checksum = 0;
        checksum ^= c;
        c_state = HEADER_SIZE;  
      }
    } 
    else if (c_state == HEADER_SIZE) {
      commandMW = c;
      checksum ^= c;
      c_state = HEADER_CMD;
    } 
    else if (c_state == HEADER_CMD && offset < dataSize) {
      checksum ^= c;
      inBuf[offset++] = c;
    } 
    else if (c_state == HEADER_CMD && offset >= dataSize) {

      if (checksum == c) {
        if (commandMW == MSP_ATTITUDE) {
          XYAngle result = p.evalAtt(inBuf);
          angX = result.angleX;
          angY = result.angleY;
          angH = result.heading;
        }
        if (commandMW == MSP_RAW_IMU) {
          IMUValues result = p.evalIMU(inBuf);
          //logger.logIMU(result);
        }
        
        if (commandMW == MSP_RC) {
          RCInput result = p.evalRC(inBuf);
          roll1 = result.roll;
          pitch1 = result.pitch;
          yaw1 = result.yaw;
          throttle1 = result.throttle;  
        }

        if (commandMW == MSP_MOTOR ) {
          MotorValues result = p.evalMotor(inBuf);
          M1 = result.motor1;
          M2 = result.motor2;
          M3 = result.motor3;
          M4 = result.motor4;
        }
        
        if (commandMW == MSP_RAW_GPS) {
          GPSValues result = p.evalGPS(inBuf);
          //logger.logGPS(result);
        }
      } 
      c_state = IDLE;
    }
  }
}
Program sprawdzony na Cleanflight, Betaflight i iNav. Wszędzie to samo. Dane z IMU umie pobrać jednak nie jestem w stanie ustawić jakieś wartości na kanale radia.
Zmieniałem jeszcze w programach (Cleanflight, Betaflight, iNav) rubrykę Configuration->Receiver na MSP RX input.
Liczę na wasze pomysły ;-)
Ostatnio zmieniony wtorek 03 lip 2018, 17:26 przez IceMAN92q, łącznie zmieniany 1 raz.
Awatar użytkownika
pawelsky
Posty: 9750
Rejestracja: środa 19 mar 2014, 02:03
Lokalizacja: Polska
Kontakt:

Re: Komunikacja Matek F405 z Arduino po UART

Post autor: pawelsky »

Po pierwsze primo delay zuoooo. Jego uzywanie w loopie powinno byc prawnie zakazane.
Po drugie jakiego Arduino uzywasz? 2560? Teensy?
Po trzecie podaj namiary na biblioteke do MSP ktorej uzywasz.
IceMAN92q
Posty: 13
Rejestracja: niedziela 14 lut 2016, 17:52
Lokalizacja: Dobra

Re: Komunikacja Matek F405 z Arduino po UART

Post autor: IceMAN92q »

Dzięki za słuszną uwagę, co używać zatem zamiast delay?
Korzystam z Adafruit Feather STM32F205. Da się go programować jak klasyczne Arduino.
Dane wysyłałam wykorzystując funkcję

Kod: Zaznacz cały

// Send a message using the MultiWii serial protocol.
void send_msp(uint8_t opcode, uint8_t * data, uint8_t n_bytes) {
  uint8_t checksum = 0;
  
  // Send the MSP header and message length
  Serial2.write((byte *)"$M<", 3);
  Serial2.write(n_bytes);
  checksum ^= n_bytes;

  // Send the op-code
  Serial2.write(opcode);
  checksum ^= opcode;
  
  // Send the data bytes
  for(int i = 0; i < n_bytes; i++) {
    Serial2.write(data[i]);
    checksum ^= data[i];
  }
  
  // Send the checksum
  Serial2.write(checksum);
}
(źródło: https://github.com/cstubens/MultiWiiSer ... serial.ino)

Próbowałęm nawet wysłać dane na sztywno zgodnie z wzorcem (http://www.multiwii.com/wiki/index.php? ... l_Protocol)

Kod: Zaznacz cały

  Serial2.write((byte *)"$M>", 3);
  Serial2.write((byte *)"16", 2);
  Serial2.write((byte *)"200", 3);
  Serial2.write((byte *)"1600, 1600, 1600, 1600, 1600, 1600, 1600, 1600", 54);
Awatar użytkownika
pawelsky
Posty: 9750
Rejestracja: środa 19 mar 2014, 02:03
Lokalizacja: Polska
Kontakt:

Re: Komunikacja Matek F405 z Arduino po UART

Post autor: pawelsky »

IceMAN92q pisze:Dzięki za słuszną uwagę, co używać zatem zamiast delay?

https://playground.arduino.cc/Code/AvoidDelay
IceMAN92q pisze:Dane wysyłałam wykorzystując funkcję

Przelacz sie z Serial2 an Serial, podepnij sie do PCta i sprawdz w jakims PuTTY albo czyms podobnym co dokladnie (binarnie) jest wysylane przez send_msp
IceMAN92q pisze:Próbowałęm nawet wysłać dane na sztywno zgodnie z wzorcem (http://www.multiwii.com/wiki/index.php? ... l_Protocol)

Kod: Zaznacz cały

  Serial2.write((byte *)"$M>", 3);
  Serial2.write((byte *)"16", 2);
  Serial2.write((byte *)"200", 3);
  Serial2.write((byte *)"1600, 1600, 1600, 1600, 1600, 1600, 1600, 1600", 54);
To akurat nie ma prawa dzialac bo wysylasz dane jako string (i do tego z przecinkami i spacjami) a powinienes jako ciag danych binarnych.
ODPOWIEDZ