mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 04:45:24 +03:00
synced serial protocol to multiwii-dev 20120622
changed booleans to bitfield struct to match with 0622 no other functional changes, and not all enhancements (like boxlight) from 0622 are implemented yet NOT flight tested, use at your own risk. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@172 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop
parent
2fd5645dce
commit
9c2204c179
11 changed files with 5584 additions and 5472 deletions
548
src/serial.c
548
src/serial.c
|
|
@ -1,6 +1,10 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
// Multiwii Serial Protocol 0
|
||||
#define MSP_VERSION 0
|
||||
#define PLATFORM_32BIT 0x80000000
|
||||
|
||||
#define MSP_IDENT 100 //out message multitype + version
|
||||
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation
|
||||
#define MSP_RAW_IMU 102 //out message 9 DOF
|
||||
|
|
@ -17,6 +21,8 @@
|
|||
#define MSP_BOX 113 //out message up to 16 checkbox (11 are used)
|
||||
#define MSP_MISC 114 //out message powermeter trig + 8 free for future use
|
||||
#define MSP_MOTOR_PINS 115 //out message which pins are in use for motors & servos, for GUI
|
||||
#define MSP_BOXNAMES 116 //out message the aux switch names
|
||||
#define MSP_PIDNAMES 117 //out message the PID names
|
||||
|
||||
#define MSP_SET_RAW_RC 200 //in message 8 rc chan
|
||||
#define MSP_SET_RAW_GPS 201 //in message fix, numsat, lat, lon, alt, speed
|
||||
|
|
@ -32,7 +38,38 @@
|
|||
|
||||
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
|
||||
|
||||
static uint8_t checksum, stateMSP, indRX, inBuf[64];
|
||||
#define INBUF_SIZE 64
|
||||
|
||||
static const char boxnames[] =
|
||||
"ACC;"
|
||||
"BARO;"
|
||||
"MAG;"
|
||||
"CAMSTAB;"
|
||||
"CAMTRIG;"
|
||||
"ARM;"
|
||||
"GPS HOME;"
|
||||
"GPS HOLD;"
|
||||
"PASSTHRU;"
|
||||
"HEADFREE;"
|
||||
"BEEPER;"
|
||||
"LEDMAX;"
|
||||
"LLIGHTS;"
|
||||
"HEADADJ;";
|
||||
|
||||
static const char pidnames[] =
|
||||
"ROLL;"
|
||||
"PITCH;"
|
||||
"YAW;"
|
||||
"ALT;"
|
||||
"Pos;"
|
||||
"PosR;"
|
||||
"NavR;"
|
||||
"LEVEL;"
|
||||
"MAG;"
|
||||
"VEL;";
|
||||
|
||||
static uint8_t checksum, indRX, inBuf[INBUF_SIZE];
|
||||
static uint8_t cmdMSP;
|
||||
|
||||
void serialize32(uint32_t a)
|
||||
{
|
||||
|
|
@ -68,35 +105,43 @@ void serialize8(uint8_t a)
|
|||
checksum ^= a;
|
||||
}
|
||||
|
||||
uint32_t read32(void)
|
||||
{
|
||||
uint32_t t = inBuf[indRX++];
|
||||
t += inBuf[indRX++] << 8;
|
||||
t += (uint32_t) inBuf[indRX++] << 16;
|
||||
t += (uint32_t) inBuf[indRX++] << 24;
|
||||
return t;
|
||||
}
|
||||
|
||||
uint16_t read16(void)
|
||||
{
|
||||
uint16_t t = inBuf[indRX++];
|
||||
t += inBuf[indRX++] << 8;
|
||||
return t;
|
||||
}
|
||||
|
||||
uint8_t read8(void)
|
||||
{
|
||||
return inBuf[indRX++] & 0xff;
|
||||
}
|
||||
|
||||
void headSerialReply(uint8_t c, uint8_t s)
|
||||
uint16_t read16(void)
|
||||
{
|
||||
uint16_t t = read8();
|
||||
t += (uint16_t) read8() << 8;
|
||||
return t;
|
||||
}
|
||||
|
||||
uint32_t read32(void)
|
||||
{
|
||||
uint32_t t = read16();
|
||||
t += (uint32_t) read16() << 16;
|
||||
return t;
|
||||
}
|
||||
|
||||
void headSerialResponse(uint8_t err, uint8_t s)
|
||||
{
|
||||
serialize8('$');
|
||||
serialize8('M');
|
||||
serialize8('>');
|
||||
serialize8(err ? '!' : '>');
|
||||
checksum = 0; // start calculating a new checksum
|
||||
serialize8(s);
|
||||
serialize8(c);
|
||||
checksum = 0;
|
||||
serialize8(cmdMSP);
|
||||
}
|
||||
|
||||
void headSerialReply(uint8_t s)
|
||||
{
|
||||
headSerialResponse(0, s);
|
||||
}
|
||||
|
||||
void headSerialError(uint8_t s)
|
||||
{
|
||||
headSerialResponse(1, s);
|
||||
}
|
||||
|
||||
void tailSerialReply(void)
|
||||
|
|
@ -106,6 +151,13 @@ void tailSerialReply(void)
|
|||
// UartSendData();
|
||||
}
|
||||
|
||||
void serializeNames(const char *s)
|
||||
{
|
||||
const char *c;
|
||||
for (c = s; *c; c++)
|
||||
serialize8(*c);
|
||||
}
|
||||
|
||||
// signal that we're in cli mode
|
||||
uint8_t cliMode = 0;
|
||||
|
||||
|
|
@ -114,10 +166,203 @@ void serialInit(uint32_t baudrate)
|
|||
uartInit(baudrate);
|
||||
}
|
||||
|
||||
static void evaluateCommand(void)
|
||||
{
|
||||
uint8_t i;
|
||||
|
||||
switch (cmdMSP) {
|
||||
case MSP_SET_RAW_RC:
|
||||
for (i = 0; i < 8; i++)
|
||||
rcData[i] = read16();
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_SET_RAW_GPS:
|
||||
f.GPS_FIX = read8();
|
||||
GPS_numSat = read8();
|
||||
GPS_coord[LAT] = read32();
|
||||
GPS_coord[LON] = read32();
|
||||
GPS_altitude = read16();
|
||||
GPS_speed = read16();
|
||||
GPS_update |= 2; // New data signalisation to GPS functions
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_SET_PID:
|
||||
for (i = 0; i < PIDITEMS; i++) {
|
||||
cfg.P8[i] = read8();
|
||||
cfg.I8[i] = read8();
|
||||
cfg.D8[i] = read8();
|
||||
}
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_SET_BOX:
|
||||
for (i = 0; i < CHECKBOXITEMS; i++)
|
||||
cfg.activate[i] = read16();
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_SET_RC_TUNING:
|
||||
cfg.rcRate8 = read8();
|
||||
cfg.rcExpo8 = read8();
|
||||
cfg.rollPitchRate = read8();
|
||||
cfg.yawRate = read8();
|
||||
cfg.dynThrPID = read8();
|
||||
cfg.thrMid8 = read8();
|
||||
cfg.thrExpo8 = read8();
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_SET_MISC:
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_IDENT:
|
||||
headSerialReply(7);
|
||||
serialize8(VERSION); // multiwii version
|
||||
serialize8(cfg.mixerConfiguration); // type of multicopter
|
||||
serialize8(MSP_VERSION); // MultiWii Serial Protocol Version
|
||||
serialize32(PLATFORM_32BIT); // "capability"
|
||||
break;
|
||||
case MSP_STATUS:
|
||||
headSerialReply(10);
|
||||
serialize16(cycleTime);
|
||||
serialize16(i2cGetErrorCounter());
|
||||
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
|
||||
serialize32(f.ACC_MODE << BOXACC | f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.ARMED << BOXARM | rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG | f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD | f.HEADFREE_MODE << BOXHEADFREE | f.PASSTHRU_MODE << BOXPASSTHRU | rcOptions[BOXBEEPERON] << BOXBEEPERON | rcOptions[BOXLEDMAX] << BOXLEDMAX | rcOptions[BOXLLIGHTS] << BOXLLIGHTS | rcOptions[BOXHEADADJ] << BOXHEADADJ);
|
||||
break;
|
||||
case MSP_RAW_IMU:
|
||||
headSerialReply(18);
|
||||
for (i = 0; i < 3; i++)
|
||||
serialize16(accSmooth[i]);
|
||||
for (i = 0; i < 3; i++)
|
||||
serialize16(gyroData[i]);
|
||||
for (i = 0; i < 3; i++)
|
||||
serialize16(magADC[i]);
|
||||
break;
|
||||
case MSP_SERVO:
|
||||
headSerialReply(16);
|
||||
for (i = 0; i < 8; i++)
|
||||
serialize16(servo[i]);
|
||||
break;
|
||||
case MSP_MOTOR:
|
||||
headSerialReply(16);
|
||||
for (i = 0; i < 8; i++)
|
||||
serialize16(motor[i]);
|
||||
break;
|
||||
case MSP_RC:
|
||||
headSerialReply(16);
|
||||
for (i = 0; i < 8; i++)
|
||||
serialize16(rcData[i]);
|
||||
break;
|
||||
case MSP_RAW_GPS:
|
||||
headSerialReply(14);
|
||||
serialize8(f.GPS_FIX);
|
||||
serialize8(GPS_numSat);
|
||||
serialize32(GPS_coord[LAT]);
|
||||
serialize32(GPS_coord[LON]);
|
||||
serialize16(GPS_altitude);
|
||||
serialize16(GPS_speed);
|
||||
break;
|
||||
case MSP_COMP_GPS:
|
||||
headSerialReply(5);
|
||||
serialize16(GPS_distanceToHome);
|
||||
serialize16(GPS_directionToHome);
|
||||
serialize8(GPS_update & 1);
|
||||
break;
|
||||
case MSP_ATTITUDE:
|
||||
headSerialReply(8);
|
||||
for (i = 0; i < 2; i++)
|
||||
serialize16(angle[i]);
|
||||
serialize16(heading);
|
||||
serialize16(headFreeModeHold);
|
||||
break;
|
||||
case MSP_ALTITUDE:
|
||||
headSerialReply(4);
|
||||
serialize32(EstAlt);
|
||||
break;
|
||||
case MSP_BAT:
|
||||
headSerialReply(3);
|
||||
serialize8(vbat);
|
||||
serialize16(0); // power meter trash
|
||||
break;
|
||||
case MSP_RC_TUNING:
|
||||
headSerialReply(7);
|
||||
serialize8(cfg.rcRate8);
|
||||
serialize8(cfg.rcExpo8);
|
||||
serialize8(cfg.rollPitchRate);
|
||||
serialize8(cfg.yawRate);
|
||||
serialize8(cfg.dynThrPID);
|
||||
serialize8(cfg.thrMid8);
|
||||
serialize8(cfg.thrExpo8);
|
||||
break;
|
||||
case MSP_PID:
|
||||
headSerialReply(3 * PIDITEMS);
|
||||
for (i = 0; i < PIDITEMS; i++) {
|
||||
serialize8(cfg.P8[i]);
|
||||
serialize8(cfg.I8[i]);
|
||||
serialize8(cfg.D8[i]);
|
||||
}
|
||||
break;
|
||||
case MSP_BOX:
|
||||
headSerialReply(2 * CHECKBOXITEMS);
|
||||
for (i = 0; i < CHECKBOXITEMS; i++)
|
||||
serialize16(cfg.activate[i]);
|
||||
break;
|
||||
case MSP_BOXNAMES:
|
||||
headSerialReply(sizeof(boxnames) - 1);
|
||||
serializeNames(boxnames);
|
||||
break;
|
||||
case MSP_PIDNAMES:
|
||||
headSerialReply(sizeof(pidnames) - 1);
|
||||
serializeNames(pidnames);
|
||||
break;
|
||||
case MSP_MISC:
|
||||
headSerialReply(2);
|
||||
serialize16(0); // intPowerTrigger1
|
||||
break;
|
||||
case MSP_MOTOR_PINS:
|
||||
headSerialReply(8);
|
||||
for (i = 0; i < 8; i++)
|
||||
serialize8(i + 1);
|
||||
break;
|
||||
case MSP_RESET_CONF:
|
||||
checkFirstTime(true);
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_ACC_CALIBRATION:
|
||||
calibratingA = 400;
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_MAG_CALIBRATION:
|
||||
f.CALIBRATE_MAG = 1;
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_EEPROM_WRITE:
|
||||
writeParams(0);
|
||||
headSerialReply(0);
|
||||
break;
|
||||
case MSP_DEBUG:
|
||||
headSerialReply(8);
|
||||
for (i = 0; i < 4; i++)
|
||||
serialize16(debug[i]); // 4 variables are here for general monitoring purpose
|
||||
break;
|
||||
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
|
||||
headSerialError(0);
|
||||
break;
|
||||
}
|
||||
tailSerialReply();
|
||||
}
|
||||
|
||||
|
||||
void serialCom(void)
|
||||
{
|
||||
uint8_t i, c;
|
||||
static uint8_t offset, dataSize;
|
||||
uint8_t c;
|
||||
static uint8_t offset;
|
||||
static uint8_t dataSize;
|
||||
static enum _serial_state {
|
||||
IDLE,
|
||||
HEADER_START,
|
||||
HEADER_M,
|
||||
HEADER_ARROW,
|
||||
HEADER_SIZE,
|
||||
HEADER_CMD,
|
||||
} c_state = IDLE;
|
||||
|
||||
// in cli mode, all uart stuff goes to here. enter cli mode by sending #
|
||||
if (cliMode) {
|
||||
|
|
@ -128,238 +373,41 @@ void serialCom(void)
|
|||
while (uartAvailable()) {
|
||||
c = uartRead();
|
||||
|
||||
if (stateMSP > 99) { // a message with a length indication, indicating a non null payload
|
||||
if (offset <= dataSize) { // there are still some octets to read (including checksum) to complete a full message
|
||||
if (offset < dataSize)
|
||||
checksum ^= c; // the checksum is computed, except for the last octet
|
||||
inBuf[offset++] = c;
|
||||
} else { // we have read all the payload
|
||||
if (checksum == inBuf[dataSize]) { // we check is the computed checksum is ok
|
||||
switch (stateMSP) { // if yes, then we execute different code depending on the message code. read8/16/32 will look into the inBuf buffer
|
||||
case MSP_SET_RAW_RC:
|
||||
for (i = 0; i < 8; i++) {
|
||||
rcData[i] = read16();
|
||||
}
|
||||
break;
|
||||
case MSP_SET_RAW_GPS:
|
||||
GPS_fix = read8();
|
||||
GPS_numSat = read8();
|
||||
GPS_coord[LAT] = read32();
|
||||
GPS_coord[LON] = read32();
|
||||
GPS_altitude = read16();
|
||||
GPS_speed = read16();
|
||||
GPS_update = 1;
|
||||
break;
|
||||
case MSP_SET_PID:
|
||||
for (i = 0; i < PIDITEMS; i++) {
|
||||
cfg.P8[i] = read8();
|
||||
cfg.I8[i] = read8();
|
||||
cfg.D8[i] = read8();
|
||||
}
|
||||
break;
|
||||
case MSP_SET_BOX:
|
||||
for (i = 0; i < CHECKBOXITEMS; i++) {
|
||||
cfg.activate[i] = read16();
|
||||
}
|
||||
break;
|
||||
case MSP_SET_RC_TUNING:
|
||||
cfg.rcRate8 = read8();
|
||||
cfg.rcExpo8 = read8();
|
||||
cfg.rollPitchRate = read8();
|
||||
cfg.yawRate = read8();
|
||||
cfg.dynThrPID = read8();
|
||||
cfg.thrMid8 = read8();
|
||||
cfg.thrExpo8 = read8();
|
||||
break;
|
||||
case MSP_SET_MISC:
|
||||
break;
|
||||
}
|
||||
}
|
||||
stateMSP = 0; // in any case we reset the MSP state
|
||||
}
|
||||
}
|
||||
|
||||
if (stateMSP < 5) {
|
||||
if (stateMSP == 4) { // this protocol state indicates we have a message with a lenght indication, and we read here the message code (fifth octet)
|
||||
if (c > 99) { // we check if it's a valid code (should be >99)
|
||||
stateMSP = c; // the message code is then reuse to feed the protocol state
|
||||
offset = 0;
|
||||
checksum = 0;
|
||||
indRX = 0; // and we init some values which will be used in the next loops to grasp the payload
|
||||
} else {
|
||||
stateMSP = 0; // the message code seems to be invalid. this should not happen => we reset the protocol state
|
||||
}
|
||||
}
|
||||
if (stateMSP == 3) { // here, we need to check if the fourth octet indicates a code indication (>99) or a payload lenght indication (<100)
|
||||
if (c < 100) { // a message with a length indication, indicating a non null payload
|
||||
stateMSP++; // we update the protocol state to read the next octet
|
||||
dataSize = c; // we store the payload lenght
|
||||
if (dataSize > 63)
|
||||
dataSize = 63; // in order to avoid overflow, we limit the size. this should not happen
|
||||
} else {
|
||||
switch (c) { // if we are here, the fourth octet indicates a code message
|
||||
case MSP_IDENT: // and we check message code to execute the relative code
|
||||
headSerialReply(c, 2); // we reply with an header indicating a payload lenght of 2 octets
|
||||
serialize8(VERSION); // the first octet. serialize8/16/32 is used also to compute a checksum
|
||||
serialize8(cfg.mixerConfiguration); // the second one
|
||||
tailSerialReply();
|
||||
break; // mainly to send the last octet which is the checksum
|
||||
case MSP_STATUS:
|
||||
headSerialReply(c, 8);
|
||||
serialize16(cycleTime);
|
||||
serialize16(i2cGetErrorCounter());
|
||||
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
|
||||
serialize16(accMode << BOXACC | baroMode << BOXBARO | magMode << BOXMAG | armed << BOXARM | GPSModeHome << BOXGPSHOME | GPSModeHold << BOXGPSHOLD | headFreeMode << BOXHEADFREE);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_RAW_IMU:
|
||||
headSerialReply(c, 18);
|
||||
for (i = 0; i < 3; i++)
|
||||
serialize16(accSmooth[i]);
|
||||
for (i = 0; i < 3; i++)
|
||||
serialize16(gyroData[i]);
|
||||
for (i = 0; i < 3; i++)
|
||||
serialize16(magADC[i]);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_SERVO:
|
||||
headSerialReply(c, 16);
|
||||
for (i = 0; i < 8; i++)
|
||||
serialize16(servo[i]);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_MOTOR:
|
||||
headSerialReply(c, 16);
|
||||
for (i = 0; i < 8; i++)
|
||||
serialize16(motor[i]);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_RC:
|
||||
headSerialReply(c, 16);
|
||||
for (i = 0; i < 8; i++)
|
||||
serialize16(rcData[i]);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_RAW_GPS:
|
||||
headSerialReply(c, 14);
|
||||
serialize8(GPS_fix);
|
||||
serialize8(GPS_numSat);
|
||||
serialize32(GPS_coord[LAT]);
|
||||
serialize32(GPS_coord[LON]);
|
||||
serialize16(GPS_altitude);
|
||||
serialize16(GPS_speed);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_COMP_GPS:
|
||||
headSerialReply(c, 5);
|
||||
serialize16(GPS_distanceToHome);
|
||||
serialize16(GPS_directionToHome + 180);
|
||||
serialize8(GPS_update);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_ATTITUDE:
|
||||
headSerialReply(c, 6);
|
||||
for (i = 0; i < 2; i++)
|
||||
serialize16(angle[i]);
|
||||
serialize16(heading);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_ALTITUDE:
|
||||
headSerialReply(c, 4);
|
||||
serialize32(EstAlt);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_BAT:
|
||||
headSerialReply(c, 3);
|
||||
serialize8(vbat);
|
||||
serialize16(0);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_RC_TUNING:
|
||||
headSerialReply(c, 7);
|
||||
serialize8(cfg.rcRate8);
|
||||
serialize8(cfg.rcExpo8);
|
||||
serialize8(cfg.rollPitchRate);
|
||||
serialize8(cfg.yawRate);
|
||||
serialize8(cfg.dynThrPID);
|
||||
serialize8(cfg.thrMid8);
|
||||
serialize8(cfg.thrExpo8);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_PID:
|
||||
headSerialReply(c, 3 * PIDITEMS);
|
||||
for (i = 0; i < PIDITEMS; i++) {
|
||||
serialize8(cfg.P8[i]);
|
||||
serialize8(cfg.I8[i]);
|
||||
serialize8(cfg.D8[i]);
|
||||
}
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_BOX:
|
||||
headSerialReply(c, 2 * CHECKBOXITEMS);
|
||||
for (i = 0; i < CHECKBOXITEMS; i++) {
|
||||
serialize16(cfg.activate[i]);
|
||||
}
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_MISC:
|
||||
headSerialReply(c, 2);
|
||||
serialize16(0);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_MOTOR_PINS:
|
||||
headSerialReply(c, 8);
|
||||
for (i = 0; i < 8; i++)
|
||||
serialize8(i + 1);
|
||||
tailSerialReply();
|
||||
break;
|
||||
case MSP_RESET_CONF:
|
||||
checkFirstTime(true);
|
||||
break;
|
||||
case MSP_ACC_CALIBRATION:
|
||||
calibratingA = 400;
|
||||
calibratingG = 1000; // hit gyro too
|
||||
break;
|
||||
case MSP_MAG_CALIBRATION:
|
||||
calibratingM = 1;
|
||||
break;
|
||||
case MSP_EEPROM_WRITE:
|
||||
writeParams(0);
|
||||
break;
|
||||
case MSP_DEBUG:
|
||||
headSerialReply(c, 8);
|
||||
serialize16(debug1); // 4 variables are here for general monitoring purpose
|
||||
serialize16(debug2);
|
||||
serialize16(debug3);
|
||||
serialize16(debug4);
|
||||
tailSerialReply();
|
||||
break;
|
||||
}
|
||||
stateMSP = 0; // we reset the protocol state for the next loop
|
||||
}
|
||||
} else {
|
||||
switch (c) { // header detection $MW<
|
||||
case '$':
|
||||
if (stateMSP == 0)
|
||||
stateMSP++;
|
||||
break; // first octet ok, no need to go further
|
||||
case 'M':
|
||||
if (stateMSP == 1)
|
||||
stateMSP++;
|
||||
break; // second octet ok, no need to go further
|
||||
case '<':
|
||||
if (stateMSP == 2)
|
||||
stateMSP++;
|
||||
break; // third octet ok, no need to go further
|
||||
}
|
||||
}
|
||||
}
|
||||
if (stateMSP == 0) { // still compliant with older single octet command
|
||||
if (c_state == IDLE) {
|
||||
// still compliant with older single octet command
|
||||
// enable CLI
|
||||
if (c == '#')
|
||||
cliProcess();
|
||||
else if (c == 'R')
|
||||
systemReset(true); // reboot to bootloader
|
||||
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;
|
||||
continue;
|
||||
}
|
||||
dataSize = c;
|
||||
offset = 0;
|
||||
checksum = 0;
|
||||
indRX = 0;
|
||||
checksum ^= c;
|
||||
c_state = HEADER_SIZE; // the command is to follow
|
||||
} else if (c_state == HEADER_SIZE) {
|
||||
cmdMSP = 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) { // compare calculated and transferred checksum
|
||||
evaluateCommand(); // we got a valid packet, evaluate it
|
||||
}
|
||||
c_state = IDLE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue