Skip to content
Permalink
master
Switch branches/tags

MultiWiiVTOL/Protocol.cpp

Go to file
 
 
Cannot retrieve contributors at this time
934 lines (883 sloc) 29.5 KB
Raw Blame
#include "Arduino.h"
#include "config.h"
#include "def.h"
#include "types.h"
#include "EEPROM.h"
#include "LCD.h"
#include "Output.h"
#include "GPS.h"
#include "MultiWii.h"
#include "Serial.h"
#include "Protocol.h"
#include "RX.h"
/************************************** MultiWii Serial Protocol *******************************************************/
// Multiwii Serial Protocol 0
#define MSP_VERSION 0
//to multiwii developpers/committers : do not add new MSP messages without a proper argumentation/agreement on the forum
//range id [50-99] won't be assigned and can therefore be used for any custom multiwii fork without further MSP id conflict
#define MSP_PRIVATE 1 //in+out message to be used for a generic framework : MSP + function code (LIST/GET/SET) + data. no code yet
#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_NAV_STATUS 121 //out message Returns navigation status
#define MSP_NAV_CONFIG 122 //out message Returns navigation parameters
#define MSP_CELLS 130 //out message FRSKY Battery Cell Voltages
#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_SET_NAV_CONFIG 215 //in message Sets nav config parameters - write to the eeprom
#define MSP_SET_ACC_TRIM 239 //in message set acc angle trim values
#define MSP_ACC_TRIM 240 //out message get acc angle trim values
#define MSP_BIND 241 //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
#ifdef DEBUGMSG
#define DEBUG_MSG_BUFFER_SIZE 128
static char debug_buf[DEBUG_MSG_BUFFER_SIZE];
static uint8_t head_debug;
static uint8_t tail_debug;
static uint8_t debugmsg_available();
static void debugmsg_serialize(uint8_t l);
#endif
static uint8_t CURRENTPORT=0;
#define INBUF_SIZE 64
static uint8_t inBuf[INBUF_SIZE][UART_NUMBER];
static uint8_t checksum[UART_NUMBER];
static uint8_t indRX[UART_NUMBER];
static uint8_t cmdMSP[UART_NUMBER];
void evaluateOtherData(uint8_t sr);
void evaluateCommand(uint8_t c);
static uint8_t read8() {
return inBuf[indRX[CURRENTPORT]++][CURRENTPORT]&0xff;
}
static uint16_t read16() {
uint16_t t = read8();
t+= (uint16_t)read8()<<8;
return t;
}
static uint32_t read32() {
uint32_t t = read16();
t+= (uint32_t)read16()<<16;
return t;
}
static void serialize8(uint8_t a) {
SerialSerialize(CURRENTPORT,a);
checksum[CURRENTPORT] ^= a;
}
static void serialize16(int16_t a) {
serialize8((a ) & 0xFF);
serialize8((a>>8) & 0xFF);
}
static void serialize32(uint32_t a) {
serialize8((a ) & 0xFF);
serialize8((a>> 8) & 0xFF);
serialize8((a>>16) & 0xFF);
serialize8((a>>24) & 0xFF);
}
static void headSerialResponse(uint8_t err, uint8_t s) {
serialize8('$');
serialize8('M');
serialize8(err ? '!' : '>');
checksum[CURRENTPORT] = 0; // start calculating a new checksum
serialize8(s);
serialize8(cmdMSP[CURRENTPORT]);
}
static void headSerialReply(uint8_t s) {
headSerialResponse(0, s);
}
static void headSerialError() {
headSerialResponse(1,0);
}
static void tailSerialReply() {
serialize8(checksum[CURRENTPORT]);UartSendData(CURRENTPORT);
}
static void serializeNames(PGM_P s) {
headSerialReply(strlen_P(s));
for (PGM_P c = s; pgm_read_byte(c); c++)
serialize8(pgm_read_byte(c));
tailSerialReply();
}
static void __attribute__ ((noinline)) s_struct_w(uint8_t *cb,uint8_t siz) {
while(siz--) *cb++ = read8();
}
static void s_struct_partial(uint8_t *cb,uint8_t siz) {
while(siz--) serialize8(*cb++);
}
static void s_struct(uint8_t *cb,uint8_t siz) {
headSerialReply(siz);
s_struct_partial(cb,siz);
tailSerialReply();
}
static void mspAck() {
headSerialReply(0);tailSerialReply();
}
enum MSP_protocol_bytes {
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD
};
void serialCom() {
uint8_t c,cc,port,state,bytesTXBuff;
static uint8_t offset[UART_NUMBER];
static uint8_t dataSize[UART_NUMBER];
static uint8_t c_state[UART_NUMBER];
uint32_t timeMax; // limit max time in this function in case of GPS
timeMax = micros();
for(port=0;port<UART_NUMBER;port++) {
CURRENTPORT=port;
#define RX_COND
#if defined(SERIAL_RX) && (UART_NUMBER > 1)
#define RX_COND && (RX_SERIAL_PORT != port)
#endif
cc = SerialAvailable(port);
while (cc-- RX_COND) {
bytesTXBuff = SerialUsedTXBuff(port); // indicates the number of occupied bytes in TX buffer
if (bytesTXBuff > TX_BUFFER_SIZE - 50 ) return; // ensure there is enough free TX buffer to go further (50 bytes margin)
c = SerialRead(port);
#ifdef SUPPRESS_ALL_SERIAL_MSP
evaluateOtherData(c); // no MSP handling, so go directly
#else //SUPPRESS_ALL_SERIAL_MSP
state = c_state[port];
// regular data handling to detect and handle MSP and other data
if (state == IDLE) {
if (c=='$') state = HEADER_START;
else evaluateOtherData(c); // evaluate all other incoming serial data
} else if (state == HEADER_START) {
state = (c=='M') ? HEADER_M : IDLE;
} else if (state == HEADER_M) {
state = (c=='<') ? HEADER_ARROW : IDLE;
} else if (state == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
state = IDLE;
continue;
}
dataSize[port] = c;
checksum[port] = c;
offset[port] = 0;
indRX[port] = 0;
state = HEADER_SIZE; // the command is to follow
} else if (state == HEADER_SIZE) {
cmdMSP[port] = c;
checksum[port] ^= c;
state = HEADER_CMD;
} else if (state == HEADER_CMD) {
if (offset[port] < dataSize[port]) {
checksum[port] ^= c;
inBuf[offset[port]++][port] = c;
} else {
if (checksum[port] == c) // compare calculated and transferred checksum
evaluateCommand(cmdMSP[port]); // we got a valid packet, evaluate it
state = IDLE;
cc = 0; // no more than one MSP per port and per cycle
}
}
c_state[port] = state;
// SERIAL: try to detect a new nav frame based on the current received buffer
#if defined(GPS_SERIAL)
if (GPS_SERIAL == port) {
static uint32_t GPS_last_frame_seen; //Last gps frame seen at this time, used to detect stalled gps communication
if (GPS_newFrame(c)) {
//We had a valid GPS data frame, so signal task scheduler to switch to compute
if (GPS_update == 1) GPS_update = 0; else GPS_update = 1; //Blink GPS update
GPS_last_frame_seen = timeMax;
GPS_Frame = 1;
}
// Check for stalled GPS, if no frames seen for 1.2sec then consider it LOST
if ((timeMax - GPS_last_frame_seen) > 1200000) {
//No update since 1200ms clear fix...
f.GPS_FIX = 0;
GPS_numSat = 0;
}
}
if (micros()-timeMax>250) return; // Limit the maximum execution time of serial decoding to avoid time spike
#endif
#endif // SUPPRESS_ALL_SERIAL_MSP
} // while
} // for
}
void evaluateCommand(uint8_t c) {
uint32_t tmp=0;
switch(c) {
// adding this message as a comment will return an error status for MSP_PRIVATE (end of switch), allowing third party tools to distinguish the implementation of this message
//case MSP_PRIVATE:
// headSerialError();tailSerialReply(); // we don't have any custom msp currently, so tell the gui we do not use that
// break;
case MSP_SET_RAW_RC:
s_struct_w((uint8_t*)&rcSerial,16);
rcSerialCount = 50; // 1s transition
break;
case MSP_SET_PID:
mspAck();
s_struct_w((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
break;
case MSP_SET_BOX:
mspAck();
#if EXTAUX
s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*4);
#else
s_struct_w((uint8_t*)&conf.activate[0],CHECKBOXITEMS*2);
#endif
break;
case MSP_SET_RC_TUNING:
mspAck();
s_struct_w((uint8_t*)&conf.rcRate8,7);
break;
#if !defined(DISABLE_SETTINGS_TAB)
case MSP_SET_MISC:
struct {
uint16_t a,b,c,d,e,f;
uint32_t g;
uint16_t h;
uint8_t i,j,k,l;
} set_misc;
mspAck();
s_struct_w((uint8_t*)&set_misc,22);
#if defined(POWERMETER)
conf.powerTrigger1 = set_misc.a / PLEVELSCALE;
#endif
conf.minthrottle = set_misc.b;
#ifdef FAILSAFE
conf.failsafe_throttle = set_misc.e;
#endif
#if MAG
conf.mag_declination = set_misc.h;
#endif
#if defined(VBAT)
conf.vbatscale = set_misc.i;
conf.vbatlevel_warn1 = set_misc.j;
conf.vbatlevel_warn2 = set_misc.k;
conf.vbatlevel_crit = set_misc.l;
#endif
break;
case MSP_MISC:
struct {
uint16_t a,b,c,d,e,f;
uint32_t g;
uint16_t h;
uint8_t i,j,k,l;
} misc;
misc.a = intPowerTrigger1;
misc.b = conf.minthrottle;
misc.c = MAXTHROTTLE;
misc.d = MINCOMMAND;
#ifdef FAILSAFE
misc.e = conf.failsafe_throttle;
#else
misc.e = 0;
#endif
#ifdef LOG_PERMANENT
misc.f = plog.arm;
misc.g = plog.lifetime + (plog.armed_time / 1000000); // <- computation must be moved outside from serial
#else
misc.f = 0; misc.g =0;
#endif
#if MAG
misc.h = conf.mag_declination;
#else
misc.h = 0;
#endif
#ifdef VBAT
misc.i = conf.vbatscale;
misc.j = conf.vbatlevel_warn1;
misc.k = conf.vbatlevel_warn2;
misc.l = conf.vbatlevel_crit;
#else
misc.i = 0;misc.j = 0;misc.k = 0;misc.l = 0;
#endif
s_struct((uint8_t*)&misc,22);
break;
#endif
#if defined (DYNBALANCE)
case MSP_SET_MOTOR:
mspAck();
s_struct_w((uint8_t*)&motor,16);
break;
#endif
#ifdef MULTIPLE_CONFIGURATION_PROFILES
case MSP_SELECT_SETTING:
if(!f.ARMED) {
global_conf.currentSet = read8();
if(global_conf.currentSet>2) global_conf.currentSet = 0;
writeGlobalSet(0);
readEEPROM();
}
mspAck();
break;
#endif
case MSP_SET_HEAD:
mspAck();
s_struct_w((uint8_t*)&magHold,2);
break;
case MSP_IDENT:
struct {
uint8_t v,t,msp_v;
uint32_t cap;
} id;
id.v = VERSION;
id.t = MULTITYPE;
id.msp_v = MSP_VERSION;
id.cap = (0+BIND_CAPABLE)|DYNBAL<<2|FLAP<<3|NAVCAP<<4|EXTAUX<<5|((uint32_t)NAVI_VERSION<<28); //Navi version is stored in the upper four bits;
s_struct((uint8_t*)&id,7);
break;
case MSP_STATUS:
struct {
uint16_t cycleTime,i2c_errors_count,sensor;
uint32_t flag;
uint8_t set;
} st;
st.cycleTime = cycleTime;
st.i2c_errors_count = i2c_errors_count;
st.sensor = ACC|BARO<<1|MAG<<2|GPS<<3|SONAR<<4;
#if ACC
if(f.ANGLE_MODE) tmp |= 1<<BOXANGLE;
if(f.HORIZON_MODE) tmp |= 1<<BOXHORIZON;
#endif
#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
if(f.BARO_MODE) tmp |= 1<<BOXBARO;
#endif
if(f.MAG_MODE) tmp |= 1<<BOXMAG;
#if !defined(FIXEDWING)
#if defined(HEADFREE)
if(f.HEADFREE_MODE) tmp |= 1<<BOXHEADFREE;
if(rcOptions[BOXHEADADJ]) tmp |= 1<<BOXHEADADJ;
#endif
#endif
#if defined(SERVO_TILT) || defined(GIMBAL)|| defined(SERVO_MIX_TILT)
if(rcOptions[BOXCAMSTAB]) tmp |= 1<<BOXCAMSTAB;
#endif
#if defined(CAMTRIG)
if(rcOptions[BOXCAMTRIG]) tmp |= 1<<BOXCAMTRIG;
#endif
#if GPS
switch (f.GPS_mode) {
case GPS_MODE_HOLD:
tmp |= 1<<BOXGPSHOLD;
break;
case GPS_MODE_RTH:
tmp |= 1<<BOXGPSHOME;
break;
case GPS_MODE_NAV:
tmp |= 1<<BOXGPSNAV;
break;
}
#endif
#if defined(FIXEDWING) || defined(HELICOPTER)
if(f.PASSTHRU_MODE) tmp |= 1<<BOXPASSTHRU;
#endif
#if defined(BUZZER)
if(rcOptions[BOXBEEPERON]) tmp |= 1<<BOXBEEPERON;
#endif
#if defined(LED_FLASHER)
if(rcOptions[BOXLEDMAX]) tmp |= 1<<BOXLEDMAX;
if(rcOptions[BOXLEDLOW]) tmp |= 1<<BOXLEDLOW;
#endif
#if defined(LANDING_LIGHTS_DDR)
if(rcOptions[BOXLLIGHTS]) tmp |= 1<<BOXLLIGHTS;
#endif
#if defined(VARIOMETER)
if(rcOptions[BOXVARIO]) tmp |= 1<<BOXVARIO;
#endif
#if defined(INFLIGHT_ACC_CALIBRATION)
if(rcOptions[BOXCALIB]) tmp |= 1<<BOXCALIB;
#endif
#if defined(GOVERNOR_P)
if(rcOptions[BOXGOV]) tmp |= 1<<BOXGOV;
#endif
#if defined(OSD_SWITCH)
if(rcOptions[BOXOSD]) tmp |= 1<<BOXOSD;
#endif
if(f.ARMED) tmp |= 1<<BOXARM;
st.flag = tmp;
st.set = global_conf.currentSet;
s_struct((uint8_t*)&st,11);
break;
case MSP_RAW_IMU:
#if defined(DYNBALANCE)
for(uint8_t axis=0;axis<3;axis++) {imu.gyroData[axis]=imu.gyroADC[axis];imu.accSmooth[axis]= imu.accADC[axis];} // Send the unfiltered Gyro & Acc values to gui.
#endif
s_struct((uint8_t*)&imu,18);
break;
case MSP_SERVO:
s_struct((uint8_t*)&servo,16);
break;
case MSP_SERVO_CONF:
s_struct((uint8_t*)&conf.servoConf[0].min,56); // struct servo_conf_ is 7 bytes length: min:2 / max:2 / middle:2 / rate:1 ---- 8 servo => 8x7 = 56
break;
case MSP_SET_SERVO_CONF:
mspAck();
s_struct_w((uint8_t*)&conf.servoConf[0].min,56);
break;
case MSP_MOTOR:
s_struct((uint8_t*)&motor,16);
break;
case MSP_ACC_TRIM:
headSerialReply(4);
s_struct_partial((uint8_t*)&conf.angleTrim[PITCH],2);
s_struct_partial((uint8_t*)&conf.angleTrim[ROLL],2);
tailSerialReply();
break;
case MSP_SET_ACC_TRIM:
mspAck();
s_struct_w((uint8_t*)&conf.angleTrim[PITCH],2);
s_struct_w((uint8_t*)&conf.angleTrim[ROLL],2);
break;
case MSP_RC:
s_struct((uint8_t*)&rcData,RC_CHANS*2);
break;
#if GPS
case MSP_SET_RAW_GPS:
struct {
uint8_t a,b;
int32_t c,d;
int16_t e;
uint16_t f;
} set_set_raw_gps;
mspAck();
s_struct_w((uint8_t*)&set_set_raw_gps,14);
f.GPS_FIX = set_set_raw_gps.a;
GPS_numSat = set_set_raw_gps.b;
GPS_coord[LAT] = set_set_raw_gps.c;
GPS_coord[LON] = set_set_raw_gps.d;
GPS_altitude = set_set_raw_gps.e;
GPS_speed = set_set_raw_gps.f;
GPS_update |= 2; // New data signalisation to GPS functions
break;
case MSP_RAW_GPS:
struct {
uint8_t a,b;
int32_t c,d;
int16_t e;
uint16_t f,g;
} msp_raw_gps;
msp_raw_gps.a = f.GPS_FIX;
msp_raw_gps.b = GPS_numSat;
msp_raw_gps.c = GPS_coord[LAT];
msp_raw_gps.d = GPS_coord[LON];
msp_raw_gps.e = GPS_altitude;
msp_raw_gps.f = GPS_speed;
msp_raw_gps.g = GPS_ground_course;
s_struct((uint8_t*)&msp_raw_gps,16);
break;
case MSP_COMP_GPS:
struct {
uint16_t a;
int16_t b;
uint8_t c;
} msp_comp_gps;
msp_comp_gps.a = GPS_distanceToHome;
msp_comp_gps.b = GPS_directionToHome;
msp_comp_gps.c = GPS_update & 1;
s_struct((uint8_t*)&msp_comp_gps,5);
break;
#if defined(USE_MSP_WP)
case MSP_SET_NAV_CONFIG:
mspAck();
s_struct_w((uint8_t*)&GPS_conf,sizeof(GPS_conf));
break;
case MSP_NAV_CONFIG:
s_struct((uint8_t*)&GPS_conf,sizeof(GPS_conf));
break;
case MSP_NAV_STATUS: // to move to struct transmission
headSerialReply(7);
serialize8(f.GPS_mode);
serialize8(NAV_state);
serialize8(mission_step.action);
serialize8(mission_step.number);
serialize8(NAV_error);
serialize16( (int16_t)(target_bearing/100));
//serialize16(magHold);
tailSerialReply();
break;
case MSP_WP: // to move to struct transmission
{
uint8_t wp_no;
uint8_t flag;
bool success;
wp_no = read8(); //get the wp number
headSerialReply(21);
if (wp_no == 0) { //Get HOME coordinates
serialize8(wp_no);
serialize8(mission_step.action);
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
flag = MISSION_FLAG_HOME;
}
if (wp_no == 255) { //Get poshold coordinates
serialize8(wp_no);
serialize8(mission_step.action);
serialize32(GPS_hold[LAT]);
serialize32(GPS_hold[LON]);
flag = MISSION_FLAG_HOLD;
}
if ((wp_no>0) && (wp_no<255)) {
if (NAV_state == NAV_STATE_NONE) {
success = recallWP(wp_no);
serialize8(wp_no);
serialize8(mission_step.action);
serialize32(mission_step.pos[LAT]);
serialize32(mission_step.pos[LON]);
if (success == true) flag = mission_step.flag;
else flag = MISSION_FLAG_CRC_ERROR; //CRC error
} else {
serialize8(wp_no);
serialize8(0);
serialize32(GPS_home[LAT]);
serialize32(GPS_home[LON]);
flag = MISSION_FLAG_NAV_IN_PROG;
}
}
serialize32(mission_step.altitude);
serialize16(mission_step.parameter1);
serialize16(mission_step.parameter2);
serialize16(mission_step.parameter3);
serialize8(flag);
tailSerialReply();
}
break;
case MSP_SET_WP: // to move to struct transmission
{
uint8_t wp_no = read8(); //Get the step number
if (NAV_state == NAV_STATE_HOLD_INFINIT && wp_no == 255) { //Special case - during stable poshold we allow change the hold position
mission_step.number = wp_no;
mission_step.action = MISSION_HOLD_UNLIM;
uint8_t temp = read8();
mission_step.pos[LAT] = read32();
mission_step.pos[LON] = read32();
mission_step.altitude = read32();
mission_step.parameter1 = read16();
mission_step.parameter2 = read16();
mission_step.parameter3 = read16();
mission_step.flag = read8();
if (mission_step.altitude != 0) set_new_altitude(mission_step.altitude);
GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);
if ((wp_distance/100) >= GPS_conf.safe_wp_distance) NAV_state = NAV_STATE_NONE;
else NAV_state = NAV_STATE_WP_ENROUTE;
break;
}
if (NAV_state == NAV_STATE_NONE) { // The Nav state is not zero, so navigation is in progress, silently ignore SET_WP command)
mission_step.number = wp_no;
mission_step.action = read8();
mission_step.pos[LAT] = read32();
mission_step.pos[LON] = read32();
mission_step.altitude = read32();
mission_step.parameter1 = read16();
mission_step.parameter2 = read16();
mission_step.parameter3 = read16();
mission_step.flag = read8();
//It's not sure, that we want to do poshold change via mission planner so perhaps the next if is deletable
/*
if (mission_step.number == 255) //Set up new hold position via mission planner, It must set the action to MISSION_HOLD_INFINIT
{
if (mission_step.altitude !=0) set_new_altitude(mission_step.altitude); //Set the altitude
GPS_set_next_wp(&mission_step.pos[LAT], &mission_step.pos[LON], &GPS_coord[LAT], &GPS_coord[LON]);
NAV_state = NAV_STATE_WP_ENROUTE; //Go to that position, then it will switch to poshold unlimited when reached
}
*/
if (mission_step.number == 0) { //Set new Home position
GPS_home[LAT] = mission_step.pos[LAT];
GPS_home[LON] = mission_step.pos[LON];
}
if (mission_step.number >0 && mission_step.number<255) //Not home and not poshold, we are free to store it in the eprom
if (mission_step.number <= getMaxWPNumber()) // Do not thrash the EEPROM with invalid wp number
storeWP();
mspAck();
}
}
break;
#endif //USE_MSP_WP
#endif //GPS
case MSP_ATTITUDE:
s_struct((uint8_t*)&att,6);
break;
case MSP_ALTITUDE:
s_struct((uint8_t*)&alt,6);
break;
case MSP_ANALOG:
s_struct((uint8_t*)&analog,7);
break;
case MSP_RC_TUNING:
s_struct((uint8_t*)&conf.rcRate8,7);
break;
case MSP_PID:
s_struct((uint8_t*)&conf.pid[0].P8,3*PIDITEMS);
break;
case MSP_PIDNAMES:
serializeNames(pidnames);
break;
case MSP_BOX:
#if EXTAUX
s_struct((uint8_t*)&conf.activate[0],4*CHECKBOXITEMS);
#else
s_struct((uint8_t*)&conf.activate[0],2*CHECKBOXITEMS);
#endif
break;
case MSP_BOXNAMES:
serializeNames(boxnames);
break;
case MSP_BOXIDS:
headSerialReply(CHECKBOXITEMS);
for(uint8_t i=0;i<CHECKBOXITEMS;i++)
serialize8(pgm_read_byte(&(boxids[i])));
tailSerialReply();
break;
case MSP_MOTOR_PINS:
s_struct((uint8_t*)&PWM_PIN,8);
break;
case MSP_RESET_CONF:
if(!f.ARMED) LoadDefaults();
mspAck();
break;
case MSP_ACC_CALIBRATION:
if(!f.ARMED) calibratingA=512;
mspAck();
break;
#if MAG
case MSP_MAG_CALIBRATION:
if(!f.ARMED) f.CALIBRATE_MAG = 1;
mspAck();
break;
#endif
#if defined(SPEK_BIND)
case MSP_BIND:
spekBind();
mspAck();
break;
#endif
case MSP_EEPROM_WRITE:
writeParams(0);
mspAck();
break;
case MSP_DEBUG:
s_struct((uint8_t*)&debug,8);
break;
#ifdef DEBUGMSG
case MSP_DEBUGMSG:
{
uint8_t size = debugmsg_available();
if (size > 16) size = 16;
headSerialReply(size);
debugmsg_serialize(size);
tailSerialReply();
}
break;
#endif
default: // we do not know how to handle the (valid) message, indicate error MSP $M!
headSerialError();tailSerialReply();
break;
}
}
// evaluate all other incoming serial data
void evaluateOtherData(uint8_t sr) {
#ifndef SUPPRESS_OTHER_SERIAL_COMMANDS
#if GPS
#if !defined(I2C_GPS)
// on the GPS port, we must avoid interpreting incoming values for other commands because there is no
// protocol protection as is with MSP commands
// doing so with single chars would be prone to error.
if (CURRENTPORT == GPS_SERIAL) return;
#endif
#endif
switch (sr) {
// Note: we may receive weird characters here which could trigger unwanted features during flight.
// this could lead to a crash easily.
// Please use if (!f.ARMED) where neccessary
#ifdef LCD_CONF
case 's':
case 'S':
if (!f.ARMED) configurationLoop();
break;
#endif
#ifdef LOG_PERMANENT_SHOW_AT_L
case 'L':
if (!f.ARMED) dumpPLog(1);
break;
#endif
#if defined(LCD_TELEMETRY) && defined(LCD_TEXTSTAR)
case 'A': // button A press
toggle_telemetry(1);
break;
case 'B': // button B press
toggle_telemetry(2);
break;
case 'C': // button C press
toggle_telemetry(3);
break;
case 'D': // button D press
toggle_telemetry(4);
break;
case 'a': // button A release
case 'b': // button B release
case 'c': // button C release
case 'd': // button D release
break;
#endif
#ifdef LCD_TELEMETRY
case '0':
case '1':
case '2':
case '3':
case '4':
case '5':
case '6':
case '7':
case '8':
case '9':
#ifndef SUPPRESS_TELEMETRY_PAGE_R
case 'R':
#endif
#if defined(DEBUG) || defined(DEBUG_FREE)
case 'F':
#endif
toggle_telemetry(sr);
break;
#endif // LCD_TELEMETRY
}
#endif // SUPPRESS_OTHER_SERIAL_COMMANDS
}
void SerialWrite16(uint8_t port, int16_t val)
{
CURRENTPORT=port;
serialize16(val);UartSendData(port);
}
static unsigned char cStateLED = 0xFF;
unsigned char GetStateLED()
{
return cStateLED;
}
void serialLEDRefresh()
{
// for (unsigned char nSendRepeat = 0; nSendRepeat < 3; nSendRepeat++)
// {
serialize8('$');
serialize8('M');
serialize8('!');
checksum[CURRENTPORT] = 0; // start calculating a new checksum
serialize8(1);
serialize8(0x0A);
serialize8(cStateLED);
tailSerialReply();
// }
}
void serialLEDOn()
{
if (cStateLED != 0x01)
{
cStateLED = 0x01;
serialLEDRefresh();
}
}
void serialLEDOff()
{
if (cStateLED != 0x00)
{
cStateLED = 0x00;
serialLEDRefresh();
}
}
void serialLEDToggle()
{
if (cStateLED != 0x02)
{
cStateLED = 0x02;
serialLEDRefresh();
}
}
static unsigned char cStateBuzzer = 0x00;
//unsigned char GetStateBuzzer()
//{
// return cStateBuzzer;
//}
void serialBuzzerRefresh()
{
// for (unsigned char nSendRepeat = 0; nSendRepeat < 3; nSendRepeat++)
// {
serialize8('$');
serialize8('M');
serialize8('!');
checksum[CURRENTPORT] = 0; // start calculating a new checksum
serialize8(1);
serialize8(0x0D);
serialize8(cStateBuzzer);
tailSerialReply();
// }
}
void serialBuzzerOn()
{
if (cStateBuzzer != 0x01)
{
cStateBuzzer = 0x01;
serialBuzzerRefresh();
}
}
void serialBuzzerOff()
{
if (cStateBuzzer != 0x00)
{
cStateBuzzer = 0x00;
serialBuzzerRefresh();
}
}
#ifdef DEBUGMSG
void debugmsg_append_str(const char *str) {
while(*str) {
debug_buf[head_debug++] = *str++;
if (head_debug == DEBUG_MSG_BUFFER_SIZE) {
head_debug = 0;
}
}
}
static uint8_t debugmsg_available() {
if (head_debug >= tail_debug) {
return head_debug-tail_debug;
} else {
return head_debug + (DEBUG_MSG_BUFFER_SIZE-tail_debug);
}
}
static void debugmsg_serialize(uint8_t l) {
for (uint8_t i=0; i<l; i++) {
if (head_debug != tail_debug) {
serialize8(debug_buf[tail_debug++]);
if (tail_debug == DEBUG_MSG_BUFFER_SIZE) {
tail_debug = 0;
}
} else {
serialize8('\0');
}
}
}
#else
void debugmsg_append_str(const char *str) {};
#endif