I searched for days, I found several people posting their working code but I still have problems. I want to control an LED strip with arduino to make custom animations based on speed, attitude, throttle, etc. I'm using a Naze32 with MSP on UART port. I opened a SoftwareSerial on the Arduino to use the UART serial for debugging on screen. This code requests the Naze to send MSP_ATTITUDE data.
Code: Select all
#include <SoftwareSerial.h>
const int rxpin = 2;
const int txpin = 3;
SoftwareSerial Naze(rxpin, txpin);
#include "protocol.h"
#ifndef serialstate_h
#define serialstate_h
#include "serialstate.h"
#endif
#define INBUF_SIZE 64
static uint8_t inBuf[INBUF_SIZE];
static uint8_t checksum;
static uint8_t commandMW;
static uint8_t offset;
static uint8_t dataSize;
int8_t c,n;
void setup()
{
Naze.begin(9600);
Serial.begin(9600);
pinMode(13,OUTPUT); // LED for debugging
}
void loop()
{
uint8_t datad = 0;
uint8_t *data = & datad;
send_msp( MSP_ATTITUDE ,data, 0);
readData();
while(true);
}
void send_msp(uint8_t opcode, uint8_t * data, uint8_t n_bytes) {
uint8_t checksum = 0;
Naze.write((byte *)"$M<", 3);
Naze.write(n_bytes);
checksum ^= n_bytes;
Naze.write(opcode);
checksum ^= opcode;
Naze.write(checksum);
}So when the request for ATTITUDE is sent, I receive a stream from the Naze. To parse it, I use the following code (used by everybody):
Code: Select all
void readData() {
delay(60);
while (Naze.available()) {
byte c = Naze.read();
Serial.write(c); // Debugging
Serial.print(" ");
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;
digitalWrite(13,HIGH);
}
else if (c_state == HEADER_CMD && offset >= dataSize) {
if (checksum == c) {
if (commandMW == MSP_ATTITUDE) {
XYAngle result = evaluateAtt(inBuf);
}
}
c_state = IDLE;
}
}
}
The problem is, the received stream looks like this: (topdown, in decimal and ascii)
Code: Select all
ASCII DEC
$ 36
) 41
2 50
± 177
149
0
# 35
128
134
@ 64
Ø 216So no header, and the "else if" never gets to the point of filling the InBuffer...
Writing you guys is my last resort, please help me.
Boomer91