예제 #1
0
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];
}
예제 #2
0
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
예제 #3
0
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 *) &angle;  // 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

}
예제 #4
0
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
	}
}