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;
}
