int Xbee::get_xbee_helper(Angles& joystick_des_angles, uint8_t& joystick_thrust, uint8_t& flight_mode) { int returnVal= -10; // lseek(port, -(this->DATASIZE), SEEK_END); uint8_t data_received[DATASIZE]; int result = read(port, &data_received[0], DATASIZE); int16_t checksum_calc = checksum(data_received, DATASIZE); //printf("result: %i\n", result); //print_raw_bytes(data_received, DATASIZE); bool check1_correct = ( data_received[0] == 253); bool check2_correct = ( data_received[6] == 173); bool checksum_correct = (checksum_calc > 0); //printf("data_recieved[0]: %i\n",data_received[0]); if(result == 0) { printf("read 0 bytes \n"); // clean(); returnVal = -3; } if( (checksum_calc > 0) && ( data_received[0] == 253) && ( data_received[6] == 173) ) { unpack_joystick_data( joystick_des_angles, joystick_thrust, flight_mode, data_received); calcDt(); //printf("checksum: %i \n",checksum_calc); returnVal = 1; } else { if (result == -1) printf("get_joystick_data: FAILED read from port \n"); //printf("\n\n\x1b[31mFIRST BYTE OR CHECKSUM WRONG:FLUSHED PORT\x1b[0m\n\n"); tcflush(port, TCIFLUSH); if(checksum_calc < 0) returnVal = -4; else if(!( data_received[0] == 253)) returnVal = -5; else if(!( data_received[6] == 173)) returnVal = -6; else returnVal = -1; } return returnVal; }
int Sonar::check_first_read(void) { //Once we find start byte, we begin filling in array byte-by-byte until we get 4 bytes. We then confirm that the next byte is the last byte, if it is we then cast the array into a 32 bit integer and return. If the last byte is incorrect, we dicard array, reset index, and continue searching for start byte again. int num_fds = select(port+1, &read_fds, NULL, NULL, &no_timeout); //No data ready to read if(num_fds == 0) return -2; //select returned an error else if(num_fds ==-1) return -3; //A file descriptor is ready to read: check which one else if(FD_ISSET((this->port) , &read_fds)) { int returnVal; // cout << "in check_first_read succ!!!! \n"; char sensor_byte[1] = {255}; lseek(port, -(this->data_size), SEEK_END); //read in 1 byte of data from port to the address in memory &sensor_bytes2 result indicates success of reading //read is guarunteed not to block! but can still return 0 bytes!!!! int result = read(port, &sensor_byte[0], 1); //printf("%x \n",sensor_byte[0]); char a = sensor_byte[0]; std::cout << "result: " << result << ", "<< hex << a << std::endl; //printf("FirstByteFound %i, index %i, New byte is %d result of read: %i\n", foundFirstByte, index, sensor_byte[0], result); if(result == 0) { cout << "NO BYTES \n"; printf("Failed read: read returns no (0) bytes! Might be disconnected\n\n"); clean(); returnVal = -1; } //cout << "Index: " << index << "First Byte Flag: " << foundFirstByte << endl; //finished, found all bytes if( (foundFirstByte == true) && (index == 4) ) { cout << "FB true and index 4 \n"; //check to make sure last byte correct then return distance value //ensure to clean up array after if(sensor_byte[0] == 13) { cout << "LAST BYTE CORRECT!!!!!!!! \n"; uint32_t range_char = 0; sscanf(&dist[0], "%d", &range_char); printf("Range: %d \n\n", range_char); cout << range_char << endl; //print_raw_bytes(dist, 4); //uint32_t range_char_copy = range_char; clean(); //printf("1\n"); calcDt(); (this->last_distance) = (int) range_char; //printf("frequency of succ read: %f \n", 1/calc_dt); returnVal = range_char; } else { cout << "last byte incorrect! \n"; printf("Last byte wrong: byte is %d should be 13. index is %i \n", sensor_byte[0], index); tcflush(port, TCIFLUSH); clean(); //printf("2\n"); returnVal = -4; } } //still collecting bytes, add to array, increment index else if(foundFirstByte == true) { dist[index] = sensor_byte[0]; index++; //printf("3\n"); returnVal = -1; } //havent found first or last byte, check each byte to see if this is first else if(sensor_byte[0] == 82) { foundFirstByte = true; index = 0; //printf("4\n"); returnVal = -1; } //cout << "returnVal: " << returnVal << endl; return returnVal; } //printf("5\n"); clean(); //open_port("/dev/ttyUSB0", 1); return -1; }
int Sonar::check_first_read_test(SonarTest& s) { //Once we find start byte, we begin filling in array byte-by-byte until we get 4 bytes. We then confirm that the next byte is the last byte, if it is we then cast the array into a 32 bit integer and return. If the last byte is incorrect, we dicard array, reset index, and continue searching for start byte again. int num_fds = select(port+1, &read_fds, NULL, NULL, &no_timeout); int returnVal= -10; s.num_fds = num_fds; if(num_fds == -1) (this->num_fds_n1)++; else if (num_fds == 0) (this->num_fds_0)++; else if (num_fds == 1) (this->num_fds_1)++; else if (num_fds > 1) (this->num_fds_p)++; //No data ready to read if(num_fds == 0) { s.succ_read = -1; returnVal= -1; return -1; } //select returned an error else if(num_fds ==-1) { s.succ_read= -2; returnVal= -2; return -2; } //A file descriptor is ready to read: check which one //else if(FD_ISSET((this->port) , &read_fds)) else if(num_fds ==1) { char sensor_byte[1] = {255}; lseek(port, -(this->data_size), SEEK_END); int result = read(port, &sensor_byte[0], 1); char a = sensor_byte[0]; (this->lastByte) = a; if(result == 0) { printf("read 0 bytes \n"); clean(); returnVal = -3; } //finished, found all bytes else if( (foundFirstByte == true) && (index == 4) ) { //printf(" number of bytes read: %i, index: %i, byte: %d \n", result, index, a); if(sensor_byte[0] == 13) { uint32_t range_char = 0; sscanf(&dist[0], "%d", &range_char); //printf("read last byte 13 with index = 4 \n"); //printf(" Distance: %i \n", range_char); clean(); calcDt(); (this->last_distance) = range_char; returnVal = range_char; } else { //printf("read last byte NOT 13 with index = 4\n"); tcflush(port, TCIFLUSH); clean(); returnVal = -4; } } //still collecting bytes, add to array, increment index else if(foundFirstByte == true) { //printf(" adding to array & index++\n"); dist[index] = sensor_byte[0]; index++; returnVal = -5; } //havent found first or last byte, check each byte to see if this is first else if(sensor_byte[0] == 82) { //printf("found first byte\n"); foundFirstByte = true; index = 0; returnVal = -6; } else { returnVal = -7; clean(); } s.succ_read = returnVal; s.lastByte = sensor_byte[0]; s.distance = returnVal; s.index = index; s.foundFirstByte = foundFirstByte; s.foundLastByte = foundLastByte; return returnVal; } clean(); return -8; }
int Xbee::check_start_thrust(void) { if((this->port) < 0) { printf("!!!!!NOT RECEIVING ANY JOYSTICK DATA: FAILURE TO OPEN PORT!!!!\n"); usleep(100000*2); return -2; } timespec start_time, current_time; clock_gettime(CLOCK_REALTIME,&start_time); clock_gettime(CLOCK_REALTIME,¤t_time); float elapsed_time = 0; Angles joystick_des_angles = {-10}; uint8_t joystick_thrust = 0, flight_mode = 0; //read 250 values from joystick. check if the values are reasonable to start drone. if dont recieve values for 3 seconds, return nregative values int i = 0; while(i<250) { int result = this->get_xbee_data(joystick_des_angles, joystick_thrust, flight_mode); elapsed_time += calcDt(start_time, current_time); //printf("result: %i \n", result); if(result > 0) { i++; if(joystick_thrust > 30 ) { printf("!!!!!JOYSTICK HAS TO MUCH INITIAL THRUST (%i) - SYSTEM_RUN SET TO FALSE \n", joystick_thrust); this->printData(joystick_des_angles, joystick_thrust, flight_mode); usleep(1000000*2); return -1; } else { if(elapsed_time > 3) { printf("!!!!!NOT RECEIVING ANY JOYSTICK DATA!!!!\n"); usleep(100000*2); return -2; } } } if(elapsed_time > 3) { printf("!!!!!NOT RECEIVING ANY JOYSTICK DATA!!!!\n"); usleep(1000000*3); return -2; } } return 1; }
int Xbee::get_xbee_data(void) { // select returns the number of fd's ready FD_ZERO(&read_fds); FD_SET(port, &read_fds); no_timeout.tv_sec = 0; no_timeout.tv_usec = 0; int num_fds = select(port+1, &read_fds, NULL, NULL, &no_timeout); int returnVal= -10; if(num_fds == -1) (this->num_fds_n1)++; else if (num_fds == 0) (this->num_fds_0)++; else if (num_fds == 1) (this->num_fds_1)++; else if (num_fds > 1) (this->num_fds_p)++; //No data ready to read if(num_fds == 0) { returnVal= -1; return -1; } //select returned an error else if(num_fds ==-1) { returnVal= -2; return -2; } else if(num_fds ==1) { lseek(port, -(this->DATASIZE), SEEK_END); uint8_t data_received[DATASIZE]; int result = read(port, &data_received[0], DATASIZE); int16_t checksum_calc = checksum(data_received, DATASIZE); //printf("result: %i\n", result); //print_raw_bytes(data_received, DATASIZE); //CHECK BYTES MAY BE DIFFERENT FOR VICON bool check1_correct = ( data_received[0] == 253); //bool check2_correct = ( data_received[DATASIZE-2] == 173); bool checksum_correct = (checksum_calc > 0); if(result == 0) { printf("read 0 bytes \n"); // clean(); returnVal = -3; } if( (checksum_calc > 0) && ( check1_correct ) ) { calcDt(); Vicon vicon = {0.0}; //unpack_vicon_data(vicon,data_received);UNCOMMENT AND FIX!!! (this->new_vicon) = vicon; (this->new_filt_vicon) = filter_vicon(vicon, this->weights); (this->new_vicon) = vicon_velocity(this->new_vicon, this->old_vicon, this->getDt()); (this->new_filt_vicon_vel) = vicon_velocity(this->new_filt_vicon, this->old_filt_vicon, this->getDt()); pushback(this->new_vicon, this->old_vicon, this->old_old_vicon); pushback(this->new_filt_vicon, this->old_filt_vicon, this->old_old_filt_vicon); pushback(this->new_vicon_vel,this->old_vicon_vel, this->old_old_vicon_vel); pushback(this->new_filt_vicon_vel, this->old_filt_vicon_vel, this->old_old_filt_vicon_vel); returnVal = 1; } else { if (result == -1) printf("get_xbee_data: FAILED read from port \n"); //printf("\n\n\x1b[31mFIRST BYTE OR CHECKSUM WRONG:FLUSHED PORT\x1b[0m\n\n"); tcflush(port, TCIFLUSH); if(checksum_calc < 0) returnVal = -4; else if(!( data_received[0] == 253)) returnVal = -5; //check bytes?? else if(!( data_received[6] == 173)) returnVal = -6; //check bytes?? else returnVal = -1; } } return returnVal; }