AP_ADC_HIL::AP_ADC_HIL()
{
// gyros set to zero for calibration
setGyro(0,0);
setGyro(1,0);
setGyro(2,0);
// accels set to zero for calibration
setAccel(0,0);
setAccel(1,0);
setAccel(2,0);
// set diff press and temp to zero
setGyroTemp(0);
setPressure(0);
}
// Set one channel value
void AP_ADC_HIL::setHIL(int16_t p, int16_t q, int16_t r, int16_t gyroTemp,
int16_t aX, int16_t aY, int16_t aZ, int16_t diffPress)
{
// gyros
setGyro(0,p);
setGyro(1,q);
setGyro(2,r);
// temp
setGyroTemp(gyroTemp);
// accel
setAccel(0,aX);
setAccel(1,aY);
setAccel(2,aZ);
// differential pressure
setPressure(diffPress);
}
bool Tracks::getGyro()
{
setGyro();
int start;
std::string output;
getData(4);
usleep(_delay);
while(ros::ok())
{
readData(output,1);
if(output[0]==0x00)
break;
}
this->readData(output, 5 + 5*sensor_used);
output.insert(output.begin(),0x00);
std::string buff;
float value;
buff = output.substr(0,4);
value = dataToFloat32(buff);
std::string payload = output.substr(0,sensor_used*5+4);
unsigned short crc = (unsigned short)output[sensor_used*5+4]<<8;
crc +=output[sensor_used*5+5];
unsigned short crc_cal = getCRC16(payload);
if(crc!=crc_cal)
return false;
printf("crc: %x %x ",crc,crc_cal);
//int cast = reinterpret_cast<int>(value);
printf("Gyro : %x %x %x %x \n",(unsigned char)buff[0],(unsigned char)buff[1],(unsigned char)buff[2],(unsigned char)buff[3]);
int j=gyroX;
for(start = 4;start<(sensor_used*5+1);start+=5)
{
buff = output.substr(start,5);
value = dataToFloat32(buff);
printf("%x %x %x %x %x\t\t",(unsigned char)buff[0],(unsigned char)buff[1],(unsigned char)buff[2],(unsigned char)buff[3],(unsigned char)buff[4]);
std::cout<<value<<"\n";
_data.data[j++]=value;
}
std::cout<<std::endl;
}