uint8 WaitWiFiStr(char * MatchStr, uint16 TimeoutMs) // Wait for data matching matchstr or timeout (+save string into WifiRecData string/array return num chars rxd)
{
char CharNo=0;
char c;
char *OrigMatchStr = MatchStr; // save the original address of the string in case we dont get a match and need to reset
SerialPurge(WIFI_UART);
for(c=0; c<sizeof(WIFIRecData); c++)
WIFIRecData[c]='\0';
while(TimeoutMs--) // dont debug except where indicated or you WILL loose received characters!
{
while(SerialAvailable(WIFI_UART))
{
c = StrUCase(SerialRead(WIFI_UART));
WIFIRecData[CharNo++]=c;
if( StrUCase(*MatchStr) == c)
{
MatchStr++;
if(*MatchStr=='\0')
return 1; // debug here!
}
else
MatchStr = OrigMatchStr; //reset search
}
DelayMs(1);
}
return 0; // debug here!
}
u8 *BT_getBuffer(u8 uart_port, u8 *buffer)
{
u8 c, i = 0;
// wait until something is received
while (!SerialAvailable(uart_port));
do {
c = SerialRead(uart_port); // return 255 (-1) if no reception
if (c != 255) buffer[i++] = c;
} while (c != 255);// && (i < 80) );
// C string must be null-terminated
//BT_BUFFER[i] = '\0';
buffer[i] = '\0';
//CDCprintf("buffer = [%s]\r\n", BT_BUFFER);
//return BT_BUFFER;
return buffer;
}
void serialCom() {
uint8_t c,n;
static uint8_t offset[UART_NUMBER];
static uint8_t dataSize[UART_NUMBER];
static enum _serial_state {
IDLE,
HEADER_START,
HEADER_M,
HEADER_ARROW,
HEADER_SIZE,
HEADER_CMD,
} c_state[UART_NUMBER];// = IDLE;
for(n=0;n<UART_NUMBER;n++) {
#if !defined(PROMINI)
CURRENTPORT=n;
#endif
#define GPS_COND
#if defined(GPS_SERIAL)
#if defined(GPS_PROMINI)
#define GPS_COND
#else
#undef GPS_COND
#define GPS_COND && (GPS_SERIAL != CURRENTPORT)
#endif
#endif
#define SPEK_COND
#if defined(SPEKTRUM) && (UART_NUMBER > 1)
#define SPEK_COND && (SPEK_SERIAL_PORT != CURRENTPORT)
#endif
#define SBUS_COND
#if defined(SBUS) && (UART_NUMBER > 1)
#define SBUS_COND && (SBUS_SERIAL_PORT != CURRENTPORT)
#endif
uint8_t cc = SerialAvailable(CURRENTPORT);
while (cc-- GPS_COND SPEK_COND SBUS_COND) {
uint8_t bytesTXBuff = SerialUsedTXBuff(CURRENTPORT); // 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(CURRENTPORT);
#ifdef SUPPRESS_ALL_SERIAL_MSP
// no MSP handling, so go directly
evaluateOtherData(c);
#else
// regular data handling to detect and handle MSP and other data
if (c_state[CURRENTPORT] == IDLE) {
c_state[CURRENTPORT] = (c=='$') ? HEADER_START : IDLE;
if (c_state[CURRENTPORT] == IDLE) evaluateOtherData(c); // evaluate all other incoming serial data
} else if (c_state[CURRENTPORT] == HEADER_START) {
c_state[CURRENTPORT] = (c=='M') ? HEADER_M : IDLE;
} else if (c_state[CURRENTPORT] == HEADER_M) {
c_state[CURRENTPORT] = (c=='<') ? HEADER_ARROW : IDLE;
} else if (c_state[CURRENTPORT] == HEADER_ARROW) {
if (c > INBUF_SIZE) { // now we are expecting the payload size
c_state[CURRENTPORT] = IDLE;
continue;
}
dataSize[CURRENTPORT] = c;
offset[CURRENTPORT] = 0;
checksum[CURRENTPORT] = 0;
indRX[CURRENTPORT] = 0;
checksum[CURRENTPORT] ^= c;
c_state[CURRENTPORT] = HEADER_SIZE; // the command is to follow
} else if (c_state[CURRENTPORT] == HEADER_SIZE) {
cmdMSP[CURRENTPORT] = c;
checksum[CURRENTPORT] ^= c;
c_state[CURRENTPORT] = HEADER_CMD;
} else if (c_state[CURRENTPORT] == HEADER_CMD && offset[CURRENTPORT] < dataSize[CURRENTPORT]) {
checksum[CURRENTPORT] ^= c;
inBuf[offset[CURRENTPORT]++][CURRENTPORT] = c;
} else if (c_state[CURRENTPORT] == HEADER_CMD && offset[CURRENTPORT] >= dataSize[CURRENTPORT]) {
if (checksum[CURRENTPORT] == c) { // compare calculated and transferred checksum
evaluateCommand(); // we got a valid packet, evaluate it
}
c_state[CURRENTPORT] = IDLE;
cc = 0; // no more than one MSP per port and per cycle
}
#endif // SUPPRESS_ALL_SERIAL_MSP
}
}
}
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 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];
for(port=0;port<UART_NUMBER;port++) {
CURRENTPORT=port;
#define GPS_COND
#if defined(GPS_SERIAL) && (UART_NUMBER > 1)
#define GPS_COND && (GPS_SERIAL != port)
#endif
#define RX_COND
#if (defined(SPEKTRUM) || defined(SBUS) || defined(SUMD)) && (UART_NUMBER > 1)
#define RX_COND && (RX_SERIAL_PORT != port)
#endif
cc = SerialAvailable(port);
while (cc-- GPS_COND 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
// no MSP handling, so go directly
evaluateOtherData(c);
#else
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;
offset[port] = 0;
checksum[port] = 0;
indRX[port] = 0;
checksum[port] ^= c;
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(); // 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;
#endif // SUPPRESS_ALL_SERIAL_MSP
}
}
}
uint8_t GPS_NewData(void) {
uint8_t axis;
#if defined(I2C_GPS)
static uint8_t _i2c_gps_status;
//Do not use i2c_writereg, since writing a register does not work if an i2c_stop command is issued at the end
//Still investigating, however with separated i2c_repstart and i2c_write commands works... and did not caused i2c errors on a long term test.
GPS_numSat = (_i2c_gps_status & 0xf0) >> 4;
_i2c_gps_status = i2c_readReg(I2C_GPS_ADDRESS,I2C_GPS_STATUS_00); //Get status register
uint8_t *varptr;
#if defined(I2C_GPS_SONAR)
i2c_rep_start(I2C_GPS_ADDRESS<<1);
i2c_write(I2C_GPS_SONAR_ALT);
i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
varptr = (uint8_t *)&sonarAlt; // altitude (in cm? maybe)
*varptr++ = i2c_readAck();
*varptr = i2c_readNak();
#endif
if (_i2c_gps_status & I2C_GPS_STATUS_3DFIX) { //Check is we have a good 3d fix (numsats>5)
f.GPS_FIX = 1;
if (_i2c_gps_status & I2C_GPS_STATUS_NEW_DATA) { //Check about new data
GPS_Frame = 1;
i2c_rep_start(I2C_GPS_ADDRESS<<1);
i2c_write(I2C_GPS_LOCATION); //Start read from here 2x2 bytes distance and direction
i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
varptr = (uint8_t *)&GPS_coord[LAT]; // for latitude displaying
*varptr++ = i2c_readAck();
*varptr++ = i2c_readAck();
*varptr++ = i2c_readAck();
*varptr = i2c_readAck();
varptr = (uint8_t *)&GPS_coord[LON]; // for longitude displaying
*varptr++ = i2c_readAck();
*varptr++ = i2c_readAck();
*varptr++ = i2c_readAck();
*varptr = i2c_readNak();
i2c_rep_start(I2C_GPS_ADDRESS<<1);
i2c_write(I2C_GPS_GROUND_SPEED);
i2c_rep_start((I2C_GPS_ADDRESS<<1)|1);
varptr = (uint8_t *)&GPS_speed; // speed in cm/s for OSD
*varptr++ = i2c_readAck();
*varptr = i2c_readAck();
varptr = (uint8_t *)&GPS_altitude; // altitude in meters for OSD
*varptr++ = i2c_readAck();
*varptr = i2c_readAck();
varptr = (uint8_t *)&GPS_ground_course;
*varptr++ = i2c_readAck();
*varptr = i2c_readNak();
} else {
return 0;
}
} else {
f.GPS_FIX = 0;
return 0;
}
#endif
#if defined(GPS_SERIAL) || defined(GPS_FROM_OSD)
#if defined(GPS_SERIAL)
uint8_t c = SerialAvailable(GPS_SERIAL);
if (c==0) return 0;
while (c--) {
if (GPS_newFrame(SerialRead(GPS_SERIAL))) {
#elif defined(GPS_FROM_OSD)
{
if(GPS_update & 2) { // Once second bit of GPS_update is set, indicate new GPS datas is readed from OSD - all in right format.
GPS_update &= 1; // We have: GPS_fix(0-2), GPS_numSat(0-15), GPS_coord[LAT & LON](signed, in 1/10 000 000 degres), GPS_altitude(signed, in meters) and GPS_speed(in cm/s)
#endif
GPS_Frame = 1;
}
}
#endif
return 1;
}
uint8_t GPS_Compute(void) {
if (GPS_Frame == 0) return 0; else GPS_Frame = 0;
if (GPS_update == 1) GPS_update = 0; else GPS_update = 1;
if (f.GPS_FIX && GPS_numSat >= 5) {
#if !defined(DONT_RESET_HOME_AT_ARM)
if (!f.ARMED) {f.GPS_FIX_HOME = 0;}
#endif
if (!f.GPS_FIX_HOME && f.ARMED) {
GPS_reset_home_position();
}
//Apply moving average filter to GPS data
#if defined(GPS_FILTERING)
GPS_filter_index = (GPS_filter_index+1) % GPS_FILTER_VECTOR_LENGTH;
for (uint8_t axis = 0; axis< 2; axis++) {
GPS_read[axis] = GPS_coord[axis]; //latest unfiltered data is in GPS_latitude and GPS_longitude
GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t
// How close we are to a degree line ? its the first three digits from the fractions of degree
// later we use it to Check if we are close to a degree line, if yes, disable averaging,
fraction3[axis] = (GPS_read[axis]- GPS_degree[axis]*10000000) / 10000;
GPS_filter_sum[axis] -= GPS_filter[axis][GPS_filter_index];
GPS_filter[axis][GPS_filter_index] = GPS_read[axis] - (GPS_degree[axis]*10000000);
GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index];
GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis]*10000000);
if ( nav_mode == NAV_MODE_POSHOLD) { //we use gps averaging only in poshold mode...
if ( fraction3[axis]>1 && fraction3[axis]<999 ) GPS_coord[axis] = GPS_filtered[axis];
}
}
#endif
//dTnav calculation
//Time for calculating x,y speed and navigation pids
static uint32_t nav_loopTimer;
dTnav = (float)(millis() - nav_loopTimer)/ 1000.0;
nav_loopTimer = millis();
// prevent runup from bad GPS
dTnav = min(dTnav, 1.0);
//calculate distance and bearings for gui and other stuff continously - From home to copter
uint32_t dist;
int32_t dir;
GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_home[LAT],&GPS_home[LON],&dist,&dir);
GPS_distanceToHome = dist/100;
GPS_directionToHome = dir/100;
if (!f.GPS_FIX_HOME) { //If we don't have home set, do not display anything
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
}
//calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating
GPS_calc_velocity();
if (f.GPS_HOLD_MODE || f.GPS_HOME_MODE){ //ok we are navigating
//do gps nav calculations here, these are common for nav and poshold
#if defined(GPS_LEAD_FILTER)
GPS_distance_cm_bearing(&GPS_coord_lead[LAT],&GPS_coord_lead[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord_lead[LAT],&GPS_coord_lead[LON]);
#else
GPS_distance_cm_bearing(&GPS_coord[LAT],&GPS_coord[LON],&GPS_WP[LAT],&GPS_WP[LON],&wp_distance,&target_bearing);
GPS_calc_location_error(&GPS_WP[LAT],&GPS_WP[LON],&GPS_coord[LAT],&GPS_coord[LON]);
#endif
switch (nav_mode) {
case NAV_MODE_POSHOLD:
//Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_poshold();
break;
case NAV_MODE_WP:
int16_t speed = GPS_calc_desired_speed(NAV_SPEED_MAX, NAV_SLOW_NAV); //slow navigation
// use error as the desired rate towards the target
//Desired output is in nav_lat and nav_lon where 1deg inclination is 100
GPS_calc_nav_rate(speed);
//Tail control
if (NAV_CONTROLS_HEADING) {
if (NAV_TAIL_FIRST) {
magHold = wrap_18000(nav_bearing-18000)/100;
} else {
magHold = nav_bearing/100;
}
}
// Are we there yet ?(within 2 meters of the destination)
if ((wp_distance <= GPS_wp_radius) || check_missed_wp()){ //if yes switch to poshold mode
nav_mode = NAV_MODE_POSHOLD;
if (NAV_SET_TAKEOFF_HEADING) { magHold = nav_takeoff_bearing; }
}
break;
}
} //end of gps calcs
}
}
void GPS_reset_home_position(void) {
if (f.GPS_FIX && GPS_numSat >= 5) {
GPS_home[LAT] = GPS_coord[LAT];
GPS_home[LON] = GPS_coord[LON];
GPS_calc_longitude_scaling(GPS_coord[LAT]); //need an initial value for distance and bearing calc
nav_takeoff_bearing = att.heading; //save takeoff heading
//Set ground altitude
f.GPS_FIX_HOME = 1;
}
}
//reset navigation (stop the navigation processor, and clear nav)
void GPS_reset_nav(void) {
uint8_t i;
for(i=0;i<2;i++) {
nav_rated[i] = 0;
nav[i] = 0;
reset_PID(&posholdPID[i]);
reset_PID(&poshold_ratePID[i]);
reset_PID(&navPID[i]);
nav_mode = NAV_MODE_NONE;
}
}
//Get the relevant P I D values and set the PID controllers
void GPS_set_pids(void) {
posholdPID_PARAM.kP = (float)conf.pid[PIDPOS].P8/100.0;
posholdPID_PARAM.kI = (float)conf.pid[PIDPOS].I8/100.0;
posholdPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
poshold_ratePID_PARAM.kP = (float)conf.pid[PIDPOSR].P8/10.0;
poshold_ratePID_PARAM.kI = (float)conf.pid[PIDPOSR].I8/100.0;
poshold_ratePID_PARAM.kD = (float)conf.pid[PIDPOSR].D8/1000.0;
poshold_ratePID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
navPID_PARAM.kP = (float)conf.pid[PIDNAVR].P8/10.0;
navPID_PARAM.kI = (float)conf.pid[PIDNAVR].I8/100.0;
navPID_PARAM.kD = (float)conf.pid[PIDNAVR].D8/1000.0;
navPID_PARAM.Imax = POSHOLD_RATE_IMAX * 100;
}
char serial4available(void)
{
return SerialAvailable(UART4);
}
