//-------------------------------------------------------
int Compass(int mode){
if (mode == 1){
SerialWrite(port,"k\n");
return SerialRead(port,1)[0];
}
else{
SerialWrite(port,"l\n");
char *datas = SerialRead(port,2);
return (datas[0]*256+datas[1]);
}
}
char Ult(unsigned char i){
switch(i){
case 1: SerialWrite(port,"a\n");
return SerialRead(port,1)[0]; break;
case 2: SerialWrite(port,"s\n");
return SerialRead(port,1)[0]; break;
case 3: SerialWrite(port,"d\n");
return SerialRead(port,1)[0]; break;
case 4: SerialWrite(port,"f\n");
return SerialRead(port,1)[0]; break;
case 5: SerialWrite(port,"g\n");
return SerialRead(port,1)[0]; break;
}
}
char ReflexiveIR(unsigned char i){
switch(i){
case 1: SerialWrite(port,"j\n");
return SerialRead(port,1)[0]; break;
case 2: SerialWrite(port,"k\n");
return SerialRead(port,1)[0]; break;
case 3: SerialWrite(port,"l\n");
return SerialRead(port,1)[0]; break;
case 4: SerialWrite(port,"q\n");
return SerialRead(port,1)[0]; break;
case 5: SerialWrite(port,"w\n");
return SerialRead(port,1)[0]; break;
}
}
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!
}
//-------------------------------------------------------
int *BallSensors(void){
SerialWrite(port,"a\n");
char *dataChar = SerialRead(port,8);
int *i = (int*)malloc(sizeof(int)*9);
for(int x=0;x<9;x++) i[x] = (int)dataChar[x];
return i;
}
void MainWindow::on_BTN_connecter_clicked()
{
if (ui->BTN_connecter->text() == "Connecter")
{
serial = new QSerialPort;
serial->setPortName(ui->comboBox->currentText());
if (serial->open(QIODevice::ReadWrite))
{
serial->setBaudRate(QSerialPort::Baud115200);
serial->setDataBits(QSerialPort::Data8);
serial->setFlowControl(QSerialPort::NoFlowControl);
serial->setParity(QSerialPort::NoParity);
ui->label_etat_serie->setText("Connecté");
ui->BTN_connecter->setText("Déconnecter");
connect(serial,SIGNAL(readyRead()),this,SLOT(SerialRead()));
}
}
else
{
serial->close();
ui->BTN_connecter->setText("Connecter");
ui->label_etat_serie->setText("Déconnecté");
}
}
int *LineSensors(void){
SerialWrite(port,"e\n");
char *datas = SerialRead(port,18);
int *i = (int*)malloc(sizeof(int)*9);
i[0] = (((int)datas[0]<<8)+(int)datas[1])-MinValue;
i[1] = (((int)datas[2]<<8)+(int)datas[3])-MinValue;
i[2] = (((int)datas[4]<<8)+(int)datas[5])-MinValue;
i[3] = (((int)datas[6]<<8)+(int)datas[7])-MinValue;
i[4] = (((int)datas[8]<<8)+(int)datas[9])-MinValue;
i[5] = (((int)datas[10]<<8)+(int)datas[11])-MinValue;
i[6] = (((int)datas[12]<<8)+(int)datas[13])-MinValue;
i[7] = (((int)datas[14]<<8)+(int)datas[15])-MinValue;
i[8] = (((int)datas[16]<<8)+(int)datas[17])-MinValue;
for (unsigned char a=0;a<9;a++){
if (i[a] < 0) i[a] = 0;
}
return i;
}
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 PacketFinish()
{
int i, ch, count = 0;
PacketSent = 0;
SerialWrite('$');
for (i = 0; i < DataOutAddr; i++)
{
SerialWrite(DataOutBuffer[i]);
}
SerialWrite('#');
SerialWrite(hex[(DataOutCsum >> 4) & 15]);
SerialWrite(hex[DataOutCsum & 15]);
while(!chr(serport) && ((ch = SerialRead()) != '+') && (ch != '$'));
if (ch == '$')
{
ParseState = 0;
DataInAddr = 0;
ComputedCsum = 0;
}
}
char Buttons(void){
SerialWrite(port,"h\n");
return SerialRead(port,1)[0];
}
//-------------------------------------------------------
bool KickSensor(void){
SerialWrite(port,"j\n");
return SerialRead(port,1)[0];
}
tBoolean CliGet()
{
static int idx = 0 ;
static int idxMax = 0 ;
static tBoolean esc = FALSE ;
tUByte inbyte ;
tBoolean enterHit = FALSE ;
tUByte* ptr ;
int size ;
static int lineCurrent = 0 ;
static int lineBuffer = 0 ;
static int lineMax = 0 ;
static tBoolean prompt = FALSE ;
int echo = FALSE ;
ptr = &inbyte ;
size = 1 ;
if( !prompt )
{
OutString( ">" ) ;
prompt = TRUE ;
}
if( SerialRead( SERIAL_PORT_MONITOR , &inbyte , 1 ) )
{
if( inbyte == ESCAPE_KEY )
{
esc = TRUE ;
}
else if( esc )
{
if( inbyte == ESCAPE2_KEY ) /* ignore after escape */
{
inbyte = 0 ;
}
else {
switch( inbyte )
{
case UP_KEY :
inbyte = 0 ;
lineBuffer-- ;
if( lineBuffer < 0 )
{
lineBuffer = lineMax-1 ;
}
memcpy( sLineCurrent , (char*)sLineBuf[ lineBuffer ] , sizeof( sLineCurrent ) ) ;
ReplaceLine( &idx , &idxMax , (char*)sLineCurrent ) ;
break ;
case DOWN_KEY :
inbyte = 0 ;
lineBuffer++ ;
if( lineBuffer >= lineMax )
{
lineBuffer = 0 ;
}
memcpy( sLineCurrent , (char*)sLineBuf[ lineBuffer ] , sizeof( sLineCurrent ) ) ;
ReplaceLine( &idx , &idxMax , (char*)sLineCurrent ) ;
break ;
case RIGHT_KEY :
inbyte = MAP_RIGHT ;
ptr = (tUByte*)RightString ;
size = sizeof( RightString ) ;
break ;
case LEFT_KEY :
inbyte = MAP_LEFT ;
ptr = (tUByte*)LeftString ;
size = sizeof( LeftString ) ;
break ;
default :
inbyte = 0 ;
break ;
}
esc = FALSE ;
}
}
if( ( inbyte == BACKSPACE_KEY )
|| ( inbyte == ENTER_KEY )
|| ( inbyte == MAP_LEFT )
|| ( inbyte == MAP_RIGHT )
|| ( ( inbyte >= ' ' ) && ( inbyte <= '~' ) ) )
{
if( inbyte == ENTER_KEY )
{
sLineCurrent[ idxMax ] = 0 ; /* null terminate string */
idx = 0 ;
idxMax = 0 ;
enterHit = TRUE ;
memcpy( sLineBuf[ lineCurrent ] , sLineCurrent , sizeof( sLineCurrent ) ) ;
lineCurrent++ ;
if( lineCurrent >= BUFFER_LINES )
{
lineCurrent = 0 ;
}
lineBuffer = lineCurrent ;
if( lineCurrent > lineMax )
{
lineMax = lineCurrent ;
}
prompt = FALSE ;
echo = TRUE ;
}
else if( inbyte == BACKSPACE_KEY )
{
if( idx )
{
SerialWrite( SERIAL_PORT_MONITOR , (tUByte*)LeftString , sizeof( LeftString ) ) ;
OutString( " " ) ;
SerialWrite( SERIAL_PORT_MONITOR , (tUByte*)LeftString , sizeof( LeftString ) ) ;
idx-- ;
}
}
else if( inbyte == MAP_LEFT )
{
if( idx )
{
idx-- ;
echo = TRUE ;
}
}
else if( ( inbyte == MAP_RIGHT ) )
{
if( idx < idxMax )
{
idx++ ;
echo = TRUE ;
}
}
else if( idx < ( LINE_BUF_SIZE - 1 ) )
{
sLineCurrent[ idx++ ] = inbyte ;
echo = TRUE ;
}
}
}
if( echo )
{
SerialWrite( SERIAL_PORT_MONITOR , ptr , size ) ;
}
if( idx > idxMax )
{
idxMax = idx ;
}
return enterHit ;
}
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 *threadSerialPCPrinterFunction(void *message) {
int ret;
char SerialPOSBuf[BUF_MAX];
time_t CurrentTime = 0 ;
time_t LastTime = 0;
int i;
int WriteFlag = 0;
ret = pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
if (0!=ret)
{
perror("Thread pthread_setcancelstat failed");
pthread_exit(NULL);
}
ret = pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);
if (0!=ret)
{
perror("Thread pthread_setcanceltype failed");
pthread_exit(NULL);
}
Message.data_length = 0;
while(1) {
pthread_mutex_lock(&(Message.data_lock));
pthread_mutex_lock(&(Response.data_lock));
if (Response.data_length>0)
{
int ret;
printf("Write to PC %d Datas!\n", Response.data_length);
ret = SerialWrite(SerialReadfd, &(Response.data[0]), Response.data_length);
if(ret!=Response.data_length)
{
printf("Response to PC failed!");
}
else
{
Response.data_length = 0;
}
}
pthread_mutex_unlock(&(Response.data_lock));
if(Message.data_length<=0)
{
//printf("hhhhhhhhhh\n");
//memset(&(Message.data[0]), '\0', BUF_MAX);
//Message.data_length = SerialRead(SerialReadfd, &(Message.data[0]), sizeof(Message.data));
Message.data_length = SerialRead(SerialReadfd, &(Message.data[0]), sizeof(Message.data));
//printf("Data length = %d\n", Message.data_length);
if(Message.data_length > 0)
{
//#define rdtscll(val) __asm__ __volatile__("rdtsc" : "=A" (val))
int n = 0;
printf("Received %d bytes data\n", Message.data_length);
LastTime = time(NULL);
/* for(n=0; n<Message.data_length; ++n)
{
printf("%x\t", Message.data[n]);
}*/
//fflush(stdout);
WriteARecord(&Message.data[0], Message.data_length);//Write data to a file;
// LOGD("%s(%d)-%s:",__FILE__,__LINE__,__FUNCTION__);
WriteFlag = 1;
// Message.data_length = 0;
pthread_mutex_unlock(&Message.data_lock);
msSleep(10);
}
else
{
CurrentTime = time(NULL);
if (((CurrentTime - LastTime)>5)&&(1 == WriteFlag))
{
WriteFlag = 0;
WriteARecord(&Message.data[0], 0);//close the crrent file ,and creat a queue member;
printf("Creat file!\n");
}
/* printf("no data in serialread!\n"); */
pthread_mutex_unlock(&Message.data_lock);
msSleep(10);
}
}
else
{
/* printf("Length>0\n"); */
pthread_mutex_unlock(&Message.data_lock);
msSleep(10);
}
}
pthread_exit(0);
}
int SerialHandler::read(){
return SerialRead();
}
char serial4read(void)
{
return SerialRead(UART4);
}
void CompassRaw(int *Sensor1,int *Sensor2){
SerialWrite(port,"m\n");
char *datas = SerialRead(port,4);
*Sensor1 = (datas[0]*256+datas[1]);
*Sensor2 = (datas[2]*256+datas[3]);
}
int SerialInterrupt(int signal, ppc_trap_frame_t *tf)
{
int ch;
if (!chr(serport)) return 0;
Signal = signal;
RegisterSaveArea = tf;
do
{
ch = SerialRead();
if (ch == 3) /* Break in - tehe */
{
Continue = 0;
PacketWriteSignal(3);
}
else if (ch == '+')
{
/* Nothing */
}
else if (ch == '$')
{
DataInAddr = 0;
ParseState = 0;
ComputedCsum = 0;
ActualCsum = 0;
}
else if (ch == '#' && ParseState == 0)
{
ParseState = 2;
}
else if (ParseState == 0)
{
ComputedCsum += ch;
DataInBuffer[DataInAddr++] = ch;
}
else if (ParseState == 2)
{
ActualCsum = ch;
ParseState++;
}
else if (ParseState == 3)
{
ActualCsum = hex2int(ch) | (hex2int(ActualCsum) << 4);
ComputedCsum &= 255;
ParseState = -1;
if (ComputedCsum == ActualCsum)
{
ComputedCsum = 0;
DataInBuffer[DataInAddr] = 0;
DataInAddr = 0;
Continue = 0;
SerialWrite('+');
GotPacket();
}
else
SerialWrite('-');
}
else if (ParseState == -1)
SerialWrite('-');
}
while (!Continue);
return 1;
}
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;
}
void *threadSerialPrinterPCFunction(void *message) {
int ret;
char SerialPrinterBuf[BUF_MAX];
int i;
int fd;
//ret = open
ret = pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL);
if (0!=ret)
{
perror("Thread pthread_setcancelstat failed");
pthread_exit(NULL);
}
ret = pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);
if (0!=ret)
{
perror("Thread pthread_setcanceltype failed");
pthread_exit(NULL);
}
while(1) {
// memset(SerialPrinterBuf, '\0', BUF_MAX);
/* printf("LOCK"); */
pthread_mutex_lock(&Message.data_lock);
pthread_mutex_lock(&Response.data_lock);
if(Response.data_length<=0)
{
int n = 0;
Response.data_length = SerialRead(SerialWritefd, &(Response.data[0]), sizeof(Response.data));
if(Response.data_length>0)
{
printf("Receive Data length is = %d---------------------------------------------------\n", Response.data_length);
/*for(n=0; n<Response.data_length; ++n)
{
printf("%x\t", Response.data[n]);
}*/
}
/* else
{
printf("NO DATA FROM REPONSE!\n");
}*/
}
pthread_mutex_unlock(&Response.data_lock);
/* printf("Locked!"); */
//SerialWrite(SerialWritefd, "fuckfuckfuckfuckfuckfuckfuck", 28);
/* printf("Length= %d", Message.data_length); */
// ret = SerialRead(SerialWritefd, SerialPrinterBuf, sizeof(SerialPrinterBuf));
if(Message.data_length > 0)
{
int tmp = 0;
/* char tmp1[11]= "abcdeferty";//{a, b, c, d, e, f, g, g, g, g}; */
/* SerialWrite(SerialWritefd, tmp1, 10); */
/* Message.data_length = 0; */
/* pthread_mutex_unlock(&Message.data_lock); */
/* continue; */
//tmp =
//tmp = SerialWrite(SerialWritefd, Message.data, Message.data_length);
printf("in wirte!\n");
tmp = SerialWrite(SerialWritefd, Message.data, Message.data_length);
if (Message.data_length!=tmp)//write data to printer
{
printf("actual num = %d, therial num = %d\n", tmp, Message.data_length);
PRINTERR("Write wrong!\n")
//pthread_mutex_unlock(&Message.data_lock);
//pthread_exit(NULL);
}
else
{
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
}
}
}
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
}
}
}
int Pro_Hw_Recv(unsigned char *buf, int len)
{
return SerialRead(buf,len);
}