void CalibrateGyro(void)
{
int i;
int ii;
GyroOffset[0] = 0;
GyroOffset[1] = 0;
GyroOffset[2] = 0;
AngleOffset[0] = 0;
AngleOffset[1] = 0;
for (i=0;i<1000;i++)
{
/** Read sensors ******************************************************/
readSensorData();
__delay32(5000); // Without this delay, the I2C command acts funny...
accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG);
accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG);
for (ii=0;ii<3;ii++)
{
GyroOffset[ii] = GyroOffset[ii] + gyro[ii];
}
AngleOffset[0] += accXangle;
AngleOffset[1] += accYangle;
}
for (ii=0;ii<3;ii++)
{
GyroOffset[ii] = GyroOffset[ii]/1000;
}
AngleOffset[0] = AngleOffset[0]/1000.0;
AngleOffset[1] = AngleOffset[1]/1000.0;
gyroXangle = AngleOffset[0];
gyroYangle = AngleOffset[1];
compAngleX = AngleOffset[0];
compAngleY = AngleOffset[1];
}
int main(int argc, char **argv)
{
if(argc!=2)
{
printf("usage: ./therm <IP address>\n");
exit(1);
}
//get host name
char hostname[32];
hostname[31] = '\0';
gethostname(hostname,1023);
char inputFile[1024] = "/etc/t_client/client.conf";
char errorLogFile[1024] = "/var/log/therm/error/g07_error_log";
FILE *fp = fopen(inputFile,"r");
FILE *fpError = fopen(errorLogFile,"a");
if(fpError==NULL)
{
printf("Error trying to open error file\n");
exit(1);
}
if(fp==NULL)
{
char *message;
sprintf(message,"Cannot open config file %s",inputFile);
writeErrorLog(fpError,message);
exit(1);
}
char buffer[1024];
int numSensors;
//read first line of file, which holds number of sensors
if(fgets(buffer,sizeof(buffer),fp))
{
numSensors = atoi(buffer);
}
//create array of stuct Hosts
Host hosts[numSensors];
int i = 0;
//for each subsequent line, add to array of structs
while(fgets(buffer,sizeof(buffer),fp))
{
//add name of the host
strcpy(hosts[i].hostName,hostname);
//add number of thermometers to struct
hosts[i].numThermometers = numSensors;
//add sensor number (/dev/gotemp = 0 and /dev/gotemp2 = 1)
//first line would be 0 for first sensor and second line is for 2nd sensor
hosts[i].sensorNumber = i;
double low, high;
//scan for low and high values
int n = sscanf(buffer,"%lf %lf",&low,&high);
//if succesfully read two items
if(n==2)
{
//store low and high values
hosts[i].lowValue = low;
hosts[i].highValue = high;
}
else
{
#ifdef DEBUG
printf("Error reading %s\n",inputFile);
#endif
char *message;
sprintf(message,"Error reading file %s",inputFile);
writeErrorLog(fpError,message);
exit(1);
}
#ifdef DEBUG
printf("Host %i\n",i);
printf("\tName: %s\n",hosts[i].hostName);
printf("\tNum Thermometers: %d\n",hosts[i].numThermometers);
printf("\tSensor Number: %d\n",hosts[i].sensorNumber);
printf("\tLow value: %lf\n",hosts[i].lowValue);
printf("\tHigh value: %lf\n",hosts[i].highValue);
#endif
i++;
}
time_t rawtime;
struct tm *tm;
//load sensor data into structs
for(i=0;i<numSensors;i++)
{
memset((char *)&tm,0,sizeof(tm));
memset((char *)&rawtime,0,sizeof(rawtime));
int sensData;
if(readSensorData(i,&sensData,fpError)==0)
{
//put timestamp in struct
time(&rawtime);
tm = localtime(&rawtime);
strftime(hosts[i].timeStamp,32,"%Y %m %d %H %M",tm);
//put sensor data into struct
hosts[i].sensorData = sensData;
#ifdef DEBUG
printf("Sensor Data for Host %d is %lf\n",i,hosts[i].sensorData);
printf("Timestamp for Host %d is %s\n",i,hosts[i].timeStamp);
#endif
}
else
{
//error logs written in read sensor data function
exit(1);
}
}
int sockfd, n;
struct sockaddr_in servaddr;
//ip address where server is
char* ap_addr = argv[1];
//char *ap_addr = "123.4.5.6";
char* port = "9768";
//create the socket
sockfd=socket(AF_INET,SOCK_STREAM,0);
if(sockfd < 0) {
writeErrorLog(fpError,"Error opening socket");
perror("ERROR opening socket");
exit(1);
}
//build server inet address
bzero(&servaddr,sizeof(servaddr));
servaddr.sin_family = AF_INET;
servaddr.sin_addr.s_addr=inet_addr(ap_addr); //the address
servaddr.sin_port=htons(atoi(port)); //the port
//connect
if(connect(sockfd,(struct sockaddr *) &servaddr, sizeof(servaddr)) < 0) {
writeErrorLog(fpError,"Error connecting");
perror("ERROR connecting");
exit(1);
}
int send_numSensors = htonl(numSensors);
if( sendto(sockfd,&send_numSensors,sizeof(send_numSensors),0,
(struct sockaddr *) &servaddr,sizeof(servaddr)) < 0) {
writeErrorLog(fpError,"Error writing number of sensors socket");
perror("ERROR writing to socket");
exit(1);
}
//send packet of hosts to server
for(i=0;i<numSensors;i++)
{
//hosts[i].sensorData = 1000;
//set action to 0 to send
hosts[i].action = 0;
//here write to server
sendHostToServer(&sockfd,&servaddr,fpError,&hosts[i]);
}
//now find out if overtemp so send packets w/ action = 1
for(i=0;i<numSensors;i++)
{
hosts[i].action = 1;
sendHostToServer(&sockfd,&servaddr,fpError,&hosts[i]);
}
int overTemp = 0; //1 if overheating
socklen_t servlen = sizeof(servaddr);
for(i=0;i<numSensors;i++)
{
if(recvfrom(sockfd,&overTemp,sizeof(overTemp),0,(struct sockaddr*)&servaddr,&servlen)<0)
{
perror("Error receiving overtemp");
exit(1);
}
if(overTemp==1)
{
printf("SYSTEM SHUTDOWN, SENSOR %d IS OVERHEATING\n",i);
exit(1);
}
overTemp = 0;
}
close(sockfd);
fclose(fp);
fclose(fpError);
return 0;
} //end main
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
unsigned int *chptr; // For sending a float value
unsigned int command; // For sending commands to Axis Converter
double gyroXrate, gyroYrate;
char nRFstatus;
unsigned char rfCommands[32]; // Commands received from remote
unsigned char nRFregisters[10];
int iii;
long long xSpeedFilterSum = 0;
long long ySpeedFilterSum = 0;
// Toggle LED
_LATA4 = 1;
/** Read sensors ******************************************************/
readSensorData();
__delay32(1000); // Without this delay, the I2C command acts funny...
accXangle = ((atan2(accel[0], accel[2]))*RAD_TO_DEG)-AngleOffset[0];
accYangle = ((atan2(accel[1], accel[2]))*RAD_TO_DEG)-AngleOffset[1];
gyroXrate = ((double)gyro[0]-(double)GyroOffset[0])/131;
gyroYrate = -(((double)gyro[1]-(double)GyroOffset[1])/131);
/** Feedback Loop *****************************************************/
compAngleX = (compFilterGyro*(compAngleX+(gyroYrate*(double)SAMPLE_TIME)))+(compFilterAccel*accXangle); // Calculate the angle using a Complimentary filter
compAngleY = (compFilterGyro*(compAngleY+(gyroXrate*(double)SAMPLE_TIME)))+(compFilterAccel*accYangle);
if (enableSteppers == 1)
{
iCompAngleY += compAngleY;
iCompAngleX += compAngleX;
}
else
{
iCompAngleY = 0.0;
iCompAngleX = 0.0;
}
ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN+gyroXrate*(double)iGAIN);
xSpeed = -((compAngleX-sGAIN)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN+gyroYrate*(double)iGAIN);
xSpeedFilter[SpeedFilterCounter] = xSpeed;
ySpeedFilter[SpeedFilterCounter] = ySpeed;
for(iii = 0; iii<=SpeedFilterLength;iii++)
{
xSpeedFilterSum += xSpeedFilter[iii];
ySpeedFilterSum += ySpeedFilter[iii];
}
xSpeed = (long long)xSpeedFilterSum/(SpeedFilterLength+1);
ySpeed = (long long)ySpeedFilterSum/(SpeedFilterLength+1);
// ySpeed = -((compAngleY)*(double)pGAIN+(compAngleY-compAngleYlast)*(double)dGAIN);
// xSpeed = -((compAngleX)*(double)pGAIN+(compAngleX-compAngleXlast)*(double)dGAIN);
// if (xSpeed<0)
// xSpeed = -(xSpeed*xSpeed);
// else
// xSpeed = xSpeed*xSpeed;
// if (ySpeed<0)
// ySpeed = -(ySpeed*ySpeed);
// else
// ySpeed = ySpeed*ySpeed;
compAngleXlast = compAngleX;
compAngleYlast = compAngleY;
// xSpeed = gyroXrate *1000;
// ySpeed = gyroYrate *1000;
/** Build command string **********************************************/
command = 0b1000000000000000;
if (enableSteppers == 1)
command = command | 0b0100000000000000;
/* Send data to Axis Controller ***************************************/
chptr = (unsigned char *) ∠ // For sending a float value
CS_Axis = 0;
/* Command: MSB
* 15: 0 = NO Speed Values
* 14: 1 = Enable Stepper Driver
* 13:
*/
WriteSPI2(command); // Command: MSB
WriteSPI2(xSpeed); // Send x-velocity vector
WriteSPI2(ySpeed); // Send y-velocity vector
WriteSPI2(*chptr++); // Sending first part of float value
WriteSPI2(*chptr); // Sending second part of float value
CS_Axis = 1;
/** Prepare telemetry *************************************************/
setRawData2string();
/**** Telemetry string format for command=0x42 ************************/
/** command, axh, axl, ayh, ayl, azh, azl, th, tl, 9byte **/
/** gxh, gxl, gyh, gyl, gzh, gzl, cxh, cxl, cyh, cyl, czh, czl,12byte**/
/** motorStatus 1byte**/
/** --Total 22byte **/
serString[16] = xSpeed & 0xFF; // Temporary sending additional telemetry
serString[15] = xSpeed >> 8;
serString[18] = ySpeed & 0xFF;
serString[17] = ySpeed >> 8;
// serString[18] = iax & 0xFF;
// serString[17] = iax >> 8;
// serString[20] = igy & 0xFF;
// serString[19] = igy >> 8;
serString[21] = enableSteppers;
SpeedFilterCounter++;
if (SpeedFilterCounter> SpeedFilterLength)
SpeedFilterCounter = 0;
/** Receive RF commands if any ********************************************/
// LDByteWriteSPI(0x20, 0b00001010);
nRFstatus = LDBytePollnRF();
for(iii=0; iii<10;iii++) // For debug
{
nRFregisters[iii] = readnRFbyte(iii);
}
if (nRFstatus & 0b01000000) // Data Ready RX FIFO
{
// _LATB4 = 1;
//// Delay10TCYx(100);
LDByteReadSPI(0x61, rfCommands, 21); // Read RX FIFO
LDByteWriteSPI(0x27 , 0b01000000); // Clear RX FIFO interrupt bit
//ByteWriteSPI(0xE2 , 0xFF); //Flush RX FIFO
if(rfCommands[0] == 0x82)
{
if(enableSteppers)
{
enableSteppers = 0;
buttonState = 0;
//_LATB4 = 0;
} else {
enableSteppers = 1;
buttonState = 2;
//_LATB4 = 1;
}
} else if (rfCommands[0] == 0x83)
{
_LATB4 = 1;
// Set the received values to variables
for(iii=1;iii<19;iii += 3){
if(rfCommands[iii]<9)
setValueCode[rfCommands[iii]] = (rfCommands[iii+1]<<8) + rfCommands[iii+2];
}
pGAIN = setValueCode[1];
dGAIN = setValueCode[2];
compFilterGyro = (float)setValueCode[4]/(float)1000;
compFilterAccel = 1.0 - compFilterGyro;
iGAIN = (float)setValueCode[3]/100.0;
//angle = (setValueCode[6]-180)* PI /180.0;
//SpeedFilterLength = setValueCode[5];
LDByteWriteI2C(0x00D0, 0x1A, setValueCode[5]); // 0.0 ms and disable FSYNC
sGAIN = (float)(setValueCode[6]-1000)/100.0;
}
// //LATDbits.LATD3 = 0;
//
}
// LDByteWriteSPI(0x20, 0b00001010); //
// LDByteWriteSPI(0x26, 0b00000010); // Set RC_CH to reset retransmit counter
LDCommandWriteSPI(0xE2); // Flush RX buffer (not during ACK)
/** Send telemetry ********************************************************/
// _LATB15 = 0; // Set CE low to stop receiving data
// LDByteWriteSPI(0x20 , 0b00001010); // Set to send mode
// __delay32(100);
LDByteWriteSPI(0x27, 0b00010000); // clear MAX_RT (max retries)
LDCommandWriteSPI(0xE1); // Flush TX buffer (tx buffer contains last failed transmission)
LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received)
sendnRFstring( serString, 22);
/** Set nRF to recive mode ************************************************/
// __delay32(1000);
// LDByteWriteSPI(0x27, 0b00100000); // clear TX_DS (ACK received)
// LDByteWriteSPI(0x20 , 0b00001011); // Set to receive mode
// _LATB15 = 1; // Set CE high to start receiving data
/* Done with interrupt ****************************************************/
_LATA4 = 0;
_T1IF = 0; // Clear Timer 1 interrupt flag
}
int16_t main(void)
{
unsigned int buttonCounter = 20000;
int i;
/* Configure the oscillator for the device */
ConfigureOscillator();
/* Initialize IO ports and peripherals */
InitApp();
//initSerial();
initSPI1();
initSPI2();
InitI2C();
__delay32(1600000); // allow for POR for all devices
initnRF();
ControlByte = 0x00D0; // mpu6050 address
InitMPU6050(ControlByte);
// InitHMC5883L();
__delay_ms(500);
/** Init control loop *****************************************************/
readSensorData();
accXangle = (atan2(accel[0], accel[2])*RAD_TO_DEG);
accYangle = (atan2(accel[1], accel[2])*RAD_TO_DEG);
gyroXangle = accXangle;
gyroYangle = accYangle;
compAngleX = accXangle;
compAngleY = accYangle;
AngleOffset[0] = accXangle;
AngleOffset[1] = accYangle;
CalibrateGyro(); // Finding the gyro zero-offset
SetupInterrupts();
////// int serStringN = 14;
////// char nRFstatus = 0;
//////// int i;
////// delaytime = 1;
////// bool mode = 0;
while(1)
{
/** Read Button RB7 for enable steppers *******************************/
if (buttonState == 0 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 1;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 1 && !PORTBbits.RB7 && !buttonCounter)
{
enableSteppers = 2;
__delay32(40000000);
buttonState = 2;
buttonCounter = 20000;
enableSteppers = 1;
}
if (buttonState == 2 && PORTBbits.RB7 && !buttonCounter)
{
buttonState = 3;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonState == 3 && !PORTBbits.RB7 && !buttonCounter)
{
buttonState = 0;
buttonCounter = 20000;
enableSteppers = 0;
}
if (buttonCounter)
{
buttonCounter--;
}
for (i=0;i<100;i++)
{
i=i;
}
// __delay32(100);
}
}
void SensorDataManagerTask(void* pdata){
int8_t cnt = 0;
int16_t rawData[9];
int8_t err = NO_ERR;
INT8U os_err = OS_ERR_NONE;
//start
uint32_t start = alt_timestamp();
uint32_t stop = 0;
if (alt_timestamp() == 0 && alt_timestamp_start() < 0)
{
printf("ERROR starting timer");
}
while(1){
//Semaphore for periodic task
OSSemPend(sensorDataManageTaskSem, 0, &os_err);
start = alt_timestamp();
err = readSensorData(rawData); //get newRawData
acclX[cnt] = rawData[0]; //fill arrays with new raw data
acclY[cnt] = rawData[1];
acclZ[cnt] = rawData[2];
gyroX[cnt] = rawData[3] - gyroOffsets[0]; //sensor Offsets are only available for gyrodata
gyroY[cnt] = rawData[4] - gyroOffsets[1];
gyroZ[cnt] = rawData[5] - gyroOffsets[2];
compX[cnt] = rawData[6];
compY[cnt] = rawData[7];
compZ[cnt] = rawData[8];
cnt++;
if(cnt>=20){
err = avgAllArrays();
//measure sensor sampling time
stop = alt_timestamp();
averagedDataDeltaT = (stop - start) / 50; //*1000/alt_ticks_per_second(); //calculate the time needed to get all sensordata in system Ticks/ micro seconds
if (alt_timestamp() == 0 && alt_timestamp_start() < 0)
{
printf("ERROR starting timer");
}
SDM_NEW_DATA_AVAILABLE = 1; //new data is available
}
cnt = cnt % VALUE_NUM; //cnt goes from 0 to VALUE_NUM and then starts with 0 again
OSTimeDlyHMSM(0, 0, 0, 10); //uncomment if sensordata reading is to fast for the i2c interface -> set delay to appropriate value
}
}
