Beispiel #1
0
//-------------------------------------------------------
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]); 
	}
}
Beispiel #2
0
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;
  }
}
Beispiel #3
0
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!
}
Beispiel #5
0
//-------------------------------------------------------
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é");
    }
}
Beispiel #7
0
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;
}
Beispiel #9
0
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;
    }
}
Beispiel #10
0
char Buttons(void){
  SerialWrite(port,"h\n");
  return SerialRead(port,1)[0];
}
Beispiel #11
0
//-------------------------------------------------------
bool KickSensor(void){
	SerialWrite(port,"j\n");
        return SerialRead(port,1)[0];
}
Beispiel #12
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 ;
}
Beispiel #13
0
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
}
Beispiel #14
0
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);
}
Beispiel #15
0
int SerialHandler::read(){
	return SerialRead();
}
Beispiel #16
0
char serial4read(void)
{
    return SerialRead(UART4);
}
Beispiel #17
0
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]);
}
Beispiel #18
0
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;
}
Beispiel #19
0
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;
}
Beispiel #20
0
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
                    {
Beispiel #21
0
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
    }
  }
}
Beispiel #23
0
int Pro_Hw_Recv(unsigned char *buf, int len)
{
	return SerialRead(buf,len);
}