Komunikacja Matek F405 z Arduino po UART
: poniedziałek 02 lip 2018, 21:38
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
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
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;
}
}
}
Zmieniałem jeszcze w programach (Cleanflight, Betaflight, iNav) rubrykę Configuration->Receiver na MSP RX input.
Liczę na wasze pomysły