Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity
betaflight/src/serial.c

722 lines
25 KiB
C
Executable file
Raw Blame History

#include "board.h"
#include "mw.h"
// Multiwii Serial Protocol 0
#define MSP_VERSION 0
#define PLATFORM_32BIT ((uint32_t)1 << 31)
#define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
#define MSP_RAW_IMU 102 //out message 9 DOF
#define MSP_SERVO 103 //out message 8 servos
#define MSP_MOTOR 104 //out message 8 motors
#define MSP_RC 105 //out message 8 rc chan and more
#define MSP_RAW_GPS 106 //out message fix, numsat, lat, lon, alt, speed, ground course
#define MSP_COMP_GPS 107 //out message distance home, direction home
#define MSP_ATTITUDE 108 //out message 2 angles 1 heading
#define MSP_ALTITUDE 109 //out message altitude, variometer
#define MSP_ANALOG 110 //out message vbat, powermetersum, rssi if available on RX
#define MSP_RC_TUNING 111 //out message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_PID 112 //out message P I D coeff (9 are used currently)
#define MSP_BOX 113 //out message BOX setup (number is dependant of your setup)
#define MSP_MISC 114 //out message powermeter trig
#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_WP 118 //out message get a WP, WP# is in the payload, returns (WP#, lat, lon, alt, flags) WP#0-home, WP#16-poshold
#define MSP_BOXIDS 119 //out message get the permanent IDs associated to BOXes
#define MSP_SERVO_CONF 120 //out message Servo settings
#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
#define MSP_SET_PID 202 //in message P I D coeff (9 are used currently)
#define MSP_SET_BOX 203 //in message BOX setup (number is dependant of your setup)
#define MSP_SET_RC_TUNING 204 //in message rc rate, rc expo, rollpitch rate, yaw rate, dyn throttle PID
#define MSP_ACC_CALIBRATION 205 //in message no param
#define MSP_MAG_CALIBRATION 206 //in message no param
#define MSP_SET_MISC 207 //in message powermeter trig + 8 free for future use
#define MSP_RESET_CONF 208 //in message no param
#define MSP_SET_WP 209 //in message sets a given WP (WP#,lat, lon, alt, flags)
#define MSP_SELECT_SETTING 210 //in message Select Setting Number (0-2)
#define MSP_SET_HEAD 211 //in message define a new heading hold direction
#define MSP_SET_SERVO_CONF 212 //in message Servo settings
#define MSP_SET_MOTOR 214 //in message PropBalance function
// #define MSP_BIND 240 //in message no param
#define MSP_EEPROM_WRITE 250 //in message no param
#define MSP_DEBUGMSG 253 //out message debug string buffer
#define MSP_DEBUG 254 //out message debug1,debug2,debug3,debug4
// Additional commands that are not compatible with MultiWii
#define MSP_UID 160 //out message Unique device ID
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_GPSSVINFO 164 //out message get Signal Strength (only U-Blox)
#define INBUF_SIZE 64
struct box_t {
const uint8_t boxIndex; // this is from boxnames enum
const char *boxName; // GUI-readable box name
const uint8_t permanentId; //
} boxes[] = {
{ BOXARM, "ARM;", 0 },
{ BOXANGLE, "ANGLE;", 1 },
{ BOXHORIZON, "HORIZON;", 2 },
{ BOXBARO, "BARO;", 3 },
{ BOXVARIO, "VARIO;", 4 },
{ BOXMAG, "MAG;", 5 },
{ BOXHEADFREE, "HEADFREE;", 6 },
{ BOXHEADADJ, "HEADADJ;", 7 },
{ BOXCAMSTAB, "CAMSTAB;", 8 },
{ BOXCAMTRIG, "CAMTRIG;", 9 },
{ BOXGPSHOME, "GPS HOME;", 10 },
{ BOXGPSHOLD, "GPS HOLD;", 11 },
{ BOXPASSTHRU, "PASSTHRU;", 12 },
{ BOXBEEPERON, "BEEPER;", 13 },
{ BOXLEDMAX, "LEDMAX;", 14 },
{ BOXLEDLOW, "LEDLOW;", 15 },
{ BOXLLIGHTS, "LLIGHTS;", 16 },
{ BOXCALIB, "CALIB;", 17 },
{ BOXGOV, "GOVERNOR;", 18 },
{ BOXOSD, "OSD SW;", 19 },
{ CHECKBOXITEMS, NULL, 0xFF }
};
// this is calculated at startup based on enabled features.
static uint8_t availableBoxes[CHECKBOXITEMS];
// this is the number of filled indexes in above array
static uint8_t numberBoxItems = 0;
static const char boxnames[] =
"ARM;"
"ANGLE;"
"HORIZON;"
"BARO;"
"VARIO;"
"MAG;"
"HEADFREE;"
"HEADADJ;"
"CAMSTAB;"
"CAMTRIG;"
"GPS HOME;"
"GPS HOLD;"
"PASSTHRU;"
"BEEPER;"
"LEDMAX;"
"LEDLOW;"
"LLIGHTS;"
"CALIB;"
"GOVERNOR;"
"OSD SW;";
const uint8_t boxids[] = { // permanent IDs associated to boxes. This way, you can rely on an ID number to identify a BOX function.
0, // "ARM;"
1, // "ANGLE;"
2, // "HORIZON;"
3, // "BARO;"
4, // "VARIO;"
5, // "MAG;"
6, // "HEADFREE;"
7, // "HEADADJ;"
8, // "CAMSTAB;"
9, // "CAMTRIG;"
10, // "GPS HOME;"
11, // "GPS HOLD;"
12, // "PASSTHRU;"
13, // "BEEPER;"
14, // "LEDMAX;"
15, // "LEDLOW;"
16, // "LLIGHTS;"
17, // "CALIB;"
18, // "GOVERNOR;"
19, // "OSD_SWITCH;"
};
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;
static bool guiConnected = false;
// signal that we're in cli mode
uint8_t cliMode = 0;
void serialize32(uint32_t a)
{
static uint8_t t;
t = a;
uartWrite(core.mainport, t);
checksum ^= t;
t = a >> 8;
uartWrite(core.mainport, t);
checksum ^= t;
t = a >> 16;
uartWrite(core.mainport, t);
checksum ^= t;
t = a >> 24;
uartWrite(core.mainport, t);
checksum ^= t;
}
void serialize16(int16_t a)
{
static uint8_t t;
t = a;
uartWrite(core.mainport, t);
checksum ^= t;
t = a >> 8 & 0xff;
uartWrite(core.mainport, t);
checksum ^= t;
}
void serialize8(uint8_t a)
{
uartWrite(core.mainport, a);
checksum ^= a;
}
uint8_t read8(void)
{
return inBuf[indRX++] & 0xff;
}
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(err ? '!' : '>');
checksum = 0; // start calculating a new checksum
serialize8(s);
serialize8(cmdMSP);
}
void headSerialReply(uint8_t s)
{
headSerialResponse(0, s);
}
void headSerialError(uint8_t s)
{
headSerialResponse(1, s);
}
void tailSerialReply(void)
{
serialize8(checksum);
}
void serializeNames(const char *s)
{
const char *c;
for (c = s; *c; c++)
serialize8(*c);
}
void serializeBoxNamesReply(void)
{
char buf[256]; // no fucking idea
char *c;
int i, j;
memset(buf, 0, sizeof(buf));
for (i = 0; i < CHECKBOXITEMS; i++) {
for (j = 0; j < numberBoxItems; j++) {
if (boxes[i].boxIndex == availableBoxes[j])
strcat(buf, boxes[i].boxName);
}
}
headSerialReply(strlen(buf));
for (c = buf; *c; c++)
serialize8(*c);
}
void serialInit(uint32_t baudrate)
{
int idx;
core.mainport = uartOpen(USART1, NULL, baudrate, MODE_RXTX);
// TODO fix/hax
core.telemport = core.mainport;
// calculate used boxes based on features and fill availableBoxes[] array
memset(availableBoxes, 0xFF, sizeof(availableBoxes));
idx = 0;
availableBoxes[idx++] = BOXARM;
if (sensors(SENSOR_ACC)) {
availableBoxes[idx++] = BOXANGLE;
availableBoxes[idx++] = BOXHORIZON;
}
if (sensors(SENSOR_BARO)) {
availableBoxes[idx++] = BOXBARO;
if (feature(FEATURE_VARIO))
availableBoxes[idx++] = BOXVARIO;
}
if (sensors(SENSOR_MAG)) {
availableBoxes[idx++] = BOXMAG;
availableBoxes[idx++] = BOXHEADFREE;
availableBoxes[idx++] = BOXHEADADJ;
}
if (feature(FEATURE_SERVO_TILT))
availableBoxes[idx++] = BOXCAMSTAB;
if (feature(FEATURE_GPS) && sensors(SENSOR_GPS)) {
availableBoxes[idx++] = BOXGPSHOME;
availableBoxes[idx++] = BOXGPSHOLD;
}
if (mcfg.mixerConfiguration == MULTITYPE_FLYING_WING || mcfg.mixerConfiguration == MULTITYPE_AIRPLANE)
availableBoxes[idx++] = BOXPASSTHRU;
availableBoxes[idx++] = BOXBEEPERON;
if (feature(FEATURE_INFLIGHT_ACC_CAL))
availableBoxes[idx++] = BOXCALIB;
numberBoxItems = idx;
}
static void evaluateCommand(void)
{
uint32_t i, tmp;
uint8_t wp_no;
int32_t lat = 0, lon = 0, alt = 0;
switch (cmdMSP) {
case MSP_SET_RAW_RC:
for (i = 0; i < 8; i++)
rcData[i] = read16();
headSerialReply(0);
break;
case MSP_SET_ACC_TRIM:
cfg.angleTrim[PITCH] = read16();
cfg.angleTrim[ROLL] = 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 < numberBoxItems; i++)
cfg.activate[availableBoxes[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_SELECT_SETTING:
if (!f.ARMED) {
mcfg.current_profile = read8();
if (mcfg.current_profile > 2)
mcfg.current_profile = 0;
// this writes new profile index and re-reads it
writeEEPROM(0, false);
}
headSerialReply(0);
break;
case MSP_SET_HEAD:
magHold = read16();
headSerialReply(0);
break;
case MSP_IDENT:
headSerialReply(7);
serialize8(VERSION); // multiwii version
serialize8(mcfg.mixerConfiguration); // type of multicopter
serialize8(MSP_VERSION); // MultiWii Serial Protocol Version
serialize32(PLATFORM_32BIT); // "capability"
break;
case MSP_STATUS:
headSerialReply(11);
serialize16(cycleTime);
serialize16(i2cGetErrorCounter());
serialize16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
#if FUCK_MULTIWII
// OK, so you waste all the fucking time to have BOXNAMES and BOXINDEXES etc, and then you go ahead and serialize enabled shit simply by stuffing all
// the bits in order, instead of setting the enabled bits based on BOXINDEX. WHERE IS THE FUCKING LOGIC IN THIS, FUCKWADS.
serialize32(f.ANGLE_MODE << BOXANGLE | f.HORIZON_MODE << BOXHORIZON |
f.BARO_MODE << BOXBARO | f.MAG_MODE << BOXMAG | f.HEADFREE_MODE << BOXHEADFREE | rcOptions[BOXHEADADJ] << BOXHEADADJ |
rcOptions[BOXCAMSTAB] << BOXCAMSTAB | rcOptions[BOXCAMTRIG] << BOXCAMTRIG |
f.GPS_HOME_MODE << BOXGPSHOME | f.GPS_HOLD_MODE << BOXGPSHOLD |
f.PASSTHRU_MODE << BOXPASSTHRU |
rcOptions[BOXBEEPERON] << BOXBEEPERON |
rcOptions[BOXLEDMAX] << BOXLEDMAX |
rcOptions[BOXLLIGHTS] << BOXLLIGHTS |
rcOptions[BOXVARIO] << BOXVARIO |
rcOptions[BOXCALIB] << BOXCALIB |
rcOptions[BOXGOV] << BOXGOV |
rcOptions[BOXOSD] << BOXOSD |
f.ARMED << BOXARM);
#else
// Serialize the boxes in the order we delivered them
tmp = 0;
for (i = 0; i < numberBoxItems; i++) {
uint8_t val, box = availableBoxes[i];
switch (box) {
// Handle the special cases
case BOXANGLE:
val = f.ANGLE_MODE;
break;
case BOXHORIZON:
val = f.HORIZON_MODE;
break;
case BOXMAG:
val = f.MAG_MODE;
break;
case BOXBARO:
val = f.BARO_MODE;
break;
case BOXHEADFREE:
val = f.HEADFREE_MODE;
break;
case BOXGPSHOME:
val = f.GPS_HOME_MODE;
break;
case BOXGPSHOLD:
val = f.GPS_HOLD_MODE;
break;
case BOXPASSTHRU:
val = f.PASSTHRU_MODE;
break;
case BOXARM:
val = f.ARMED;
break;
default:
// These just directly rely on their RC inputs
val = rcOptions[ box ];
break;
}
tmp |= (val << i);
}
serialize32(tmp);
#endif
serialize8(mcfg.current_profile);
break;
case MSP_RAW_IMU:
headSerialReply(18);
// Retarded hack until multiwiidorks start using real units for sensor data
if (acc_1G > 1024) {
for (i = 0; i < 3; i++)
serialize16(accSmooth[i] / 8);
} else {
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(16);
serialize8(f.GPS_FIX);
serialize8(GPS_numSat);
serialize32(GPS_coord[LAT]);
serialize32(GPS_coord[LON]);
serialize16(GPS_altitude);
serialize16(GPS_speed);
serialize16(GPS_ground_course);
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(6);
serialize32(EstAlt);
serialize16(vario);
break;
case MSP_ANALOG:
headSerialReply(5);
serialize8(vbat);
serialize16(0); // power meter trash
serialize16(rssi);
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_PIDNAMES:
headSerialReply(sizeof(pidnames) - 1);
serializeNames(pidnames);
break;
case MSP_BOX:
headSerialReply(2 * numberBoxItems);
for (i = 0; i < numberBoxItems; i++)
serialize16(cfg.activate[availableBoxes[i]]);
break;
case MSP_BOXNAMES:
// headSerialReply(sizeof(boxnames) - 1);
serializeBoxNamesReply();
break;
case MSP_BOXIDS:
headSerialReply(numberBoxItems);
for (i = 0; i < numberBoxItems; i++)
serialize8(availableBoxes[i]);
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_WP:
wp_no = read8(); // get the wp number
headSerialReply(18);
if (wp_no == 0) {
lat = GPS_home[LAT];
lon = GPS_home[LON];
} else if (wp_no == 16) {
lat = GPS_hold[LAT];
lon = GPS_hold[LON];
}
serialize8(wp_no);
serialize32(lat);
serialize32(lon);
serialize32(AltHold); // altitude (cm) will come here -- temporary implementation to test feature with apps
serialize16(0); // heading will come here (deg)
serialize16(0); // time to stay (ms) will come here
serialize8(0); // nav flag will come here
break;
case MSP_SET_WP:
wp_no = read8(); //get the wp number
lat = read32();
lon = read32();
alt = read32(); // to set altitude (cm)
read16(); // future: to set heading (deg)
read16(); // future: to set time to stay (ms)
read8(); // future: to set nav flag
if (wp_no == 0) {
GPS_home[LAT] = lat;
GPS_home[LON] = lon;
f.GPS_HOME_MODE = 0; // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
f.GPS_FIX_HOME = 1;
if (alt != 0)
AltHold = alt; // temporary implementation to test feature with apps
} else if (wp_no == 16) { // OK with SERIAL GPS -- NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
GPS_hold[LAT] = lat;
GPS_hold[LON] = lon;
if (alt != 0)
AltHold = alt; // temporary implementation to test feature with apps
nav_mode = NAV_MODE_WP;
GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
}
headSerialReply(0);
break;
case MSP_RESET_CONF:
if (!f.ARMED)
checkFirstTime(true);
headSerialReply(0);
break;
case MSP_ACC_CALIBRATION:
if (!f.ARMED)
calibratingA = 400;
headSerialReply(0);
break;
case MSP_MAG_CALIBRATION:
if (!f.ARMED)
f.CALIBRATE_MAG = 1;
headSerialReply(0);
break;
case MSP_EEPROM_WRITE:
writeEEPROM(0, true);
headSerialReply(0);
break;
case MSP_DEBUG:
headSerialReply(8);
// make use of this crap, output some useful QA statistics
debug[3] = ((hse_value / 1000000) * 1000) + (SystemCoreClock / 1000000); // XX0YY [crystal clock : core clock]
for (i = 0; i < 4; i++)
serialize16(debug[i]); // 4 variables are here for general monitoring purpose
break;
// Additional commands that are not compatible with MultiWii
case MSP_ACC_TRIM:
headSerialReply(4);
serialize16(cfg.angleTrim[PITCH]);
serialize16(cfg.angleTrim[ROLL]);
break;
case MSP_UID:
headSerialReply(12);
serialize32(U_ID_0);
serialize32(U_ID_1);
serialize32(U_ID_2);
break;
case MSP_GPSSVINFO:
headSerialReply(1 + (GPS_numCh * 4));
serialize8(GPS_numCh);
for (i = 0; i < GPS_numCh; i++){
serialize8(GPS_svinfo_chn[i]);
serialize8(GPS_svinfo_svid[i]);
serialize8(GPS_svinfo_quality[i]);
serialize8(GPS_svinfo_cno[i]);
}
break;
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError(0);
break;
}
tailSerialReply();
}
// evaluate all other incoming serial data
static void evaluateOtherData(uint8_t sr)
{
switch (sr) {
case '#':
cliProcess();
break;
case 'R':
systemReset(true); // reboot to bootloader
break;
}
}
void serialCom(void)
{
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) {
cliProcess();
return;
}
while (isUartAvailable(core.mainport)) {
c = uartRead(core.mainport);
if (c_state == IDLE) {
c_state = (c == '$') ? HEADER_START : IDLE;
if (c_state == IDLE)
evaluateOtherData(c); // evaluate all other incoming serial data
} 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
guiConnected = true;
} 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;
}
}
if (!cliMode && !isUartAvailable(core.telemport) && feature(FEATURE_TELEMETRY) && f.ARMED) { // The first 2 conditions should never evaluate to true but I'm putting it here anyway - silpstream
sendTelemetry();
return;
}
}
Reference in a new issue View git blame Copy permalink