main_imu, MPU6050 , racolta_dati sono per il funzionamento dell' accelerometro. my_img_sd è una libreria per gestire i dati su un sd sulla quale vengono forniti solamente le funzioni di lettura e scrittura a blocchi i file trasmetti sono la definizione e implementazione delle funzioni del protoccolo per la gestione dell' invio dei dati con il relativo formato
Dependencies: mbed
Revision 0:a9753886e1e0, committed 2017-11-05
- Comitter:
- rattokiller
- Date:
- Sun Nov 05 14:20:26 2017 +0000
- Commit message:
- librerie utili
Changed in this revision
diff -r 000000000000 -r a9753886e1e0 MPU6050.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MPU6050.h Sun Nov 05 14:20:26 2017 +0000 @@ -0,0 +1,580 @@ +#ifndef MPU6050_H +#define MPU6050_H + +#include "mpu_setting.h" + +// Set initial input parameters +enum Ascale { + AFS_2G = 0, + AFS_4G, + AFS_8G, + AFS_16G +}; + +enum Gscale { + GFS_250DPS = 0, + GFS_500DPS, + GFS_1000DPS, + GFS_2000DPS +}; + +// Specify sensor full scale +int Gscale = GFS_250DPS; +int Ascale = AFS_4G; + +//Set up I2C, (SDA,SCL) + +//I2C i2c(I2C_SDA, I2C_SCL); + +//I2C i2c(PC_9, PA_8); /* setting port i2c imu */ +I2C i2c(PF_0, PF_1); //sda, scl -> usare la 3.3 V +//I2C i2c(PB_9,PB_8); + +DigitalOut myled(LED1); + +float aRes, gRes; // scale resolutions per LSB for the sensors + +// Pin definitions +int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins + +int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output +float ax, ay, az; // Stores the real accel value in g's +int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output +float gx, gy, gz; // Stores the real gyro value in degrees per seconds +float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer +int16_t tempCount; // Stores the real internal chip temperature in degrees Celsius +float temperature; +float SelfTest[6]; + +int delt_t = 0; // used to control display output rate +int count = 0; // used to control display output rate + +// parameters for 6 DoF sensor fusion calculations +float PI = 3.14159265358979323846f; +float GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3 +float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta +float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) +float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value +float pitch, yaw, roll; +float deltat = 0.0f; // integration interval for both filter schemes +int lastUpdate = 0, firstUpdate = 0, Now = 0; // used to calculate integration interval // used to calculate integration interval +float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion + +class MPU6050 { + + protected: + + public: + //=================================================================================================================== +//====== Set of useful function to access acceleratio, gyroscope, and temperature data +//=================================================================================================================== + + void writeByte(uint8_t address, uint8_t subAddress, uint8_t data) +{ + char data_write[2]; + data_write[0] = subAddress; + data_write[1] = data; + i2c.write(address, data_write, 2, 0); +} + + char readByte(uint8_t address, uint8_t subAddress) +{ + char data[1]; // `data` will store the register data + char data_write[1]; + data_write[0] = subAddress; + i2c.write(address, data_write, 1, 1); // no stop + i2c.read(address, data, 1, 0); + return data[0]; +} + + void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest) +{ + char data[14]; + char data_write[1]; + data_write[0] = subAddress; + i2c.write(address, data_write, 1, 1); // no stop + i2c.read(address, data, count, 0); + for(int ii = 0; ii < count; ii++) { + dest[ii] = data[ii]; + } +} + + +void getGres() { + switch (Gscale) + { + // Possible gyro scales (and their register bit settings) are: + // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + case GFS_250DPS: + gRes = 250.0/32768.0; + break; + case GFS_500DPS: + gRes = 500.0/32768.0; + break; + case GFS_1000DPS: + gRes = 1000.0/32768.0; + break; + case GFS_2000DPS: + gRes = 2000.0/32768.0; + break; + } +} + +void getAres() { + switch (Ascale) + { + // Possible accelerometer scales (and their register bit settings) are: + // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + case AFS_2G: + aRes = 2.0/32768.0; + break; + case AFS_4G: + aRes = 4.0/32768.0; + break; + case AFS_8G: + aRes = 8.0/32768.0; + break; + case AFS_16G: + aRes = 16.0/32768.0; + break; + } +} + + +void readAccelData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z accel register data stored here + readBytes(MPU6050_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array + destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; + destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; +} + +void readGyroData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + readBytes(MPU6050_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ; + destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; +} + +int16_t readTempData() +{ + uint8_t rawData[2]; // x/y/z gyro register data stored here + readBytes(MPU6050_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array + return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value +} + + + +// Configure the motion detection control for low power accelerometer mode +void LowPowerAccelOnly() +{ + +// The sensor has a high-pass filter necessary to invoke to allow the sensor motion detection algorithms work properly +// Motion detection occurs on free-fall (acceleration below a threshold for some time for all axes), motion (acceleration +// above a threshold for some time on at least one axis), and zero-motion toggle (acceleration on each axis less than a +// threshold for some time sets this flag, motion above the threshold turns it off). The high-pass filter takes gravity out +// consideration for these threshold evaluations; otherwise, the flags would be set all the time! + + uint8_t c = readByte(MPU6050_ADDRESS, PWR_MGMT_1); + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x30); // Clear sleep and cycle bits [5:6] + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x30); // Set sleep and cycle bits [5:6] to zero to make sure accelerometer is running + + c = readByte(MPU6050_ADDRESS, PWR_MGMT_2); + writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0x38); // Clear standby XA, YA, and ZA bits [3:5] + writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x00); // Set XA, YA, and ZA bits [3:5] to zero to make sure accelerometer is running + + c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0] +// Set high-pass filter to 0) reset (disable), 1) 5 Hz, 2) 2.5 Hz, 3) 1.25 Hz, 4) 0.63 Hz, or 7) Hold + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x00); // Set ACCEL_HPF to 0; reset mode disbaling high-pass filter + + c = readByte(MPU6050_ADDRESS, CONFIG); + writeByte(MPU6050_ADDRESS, CONFIG, c & ~0x07); // Clear low-pass filter bits [2:0] + writeByte(MPU6050_ADDRESS, CONFIG, c | 0x00); // Set DLPD_CFG to 0; 260 Hz bandwidth, 1 kHz rate + + c = readByte(MPU6050_ADDRESS, INT_ENABLE); + writeByte(MPU6050_ADDRESS, INT_ENABLE, c & ~0xFF); // Clear all interrupts + writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x40); // Enable motion threshold (bits 5) interrupt only + +// Motion detection interrupt requires the absolute value of any axis to lie above the detection threshold +// for at least the counter duration + writeByte(MPU6050_ADDRESS, MOT_THR, 0x80); // Set motion detection to 0.256 g; LSB = 2 mg + writeByte(MPU6050_ADDRESS, MOT_DUR, 0x01); // Set motion detect duration to 1 ms; LSB is 1 ms @ 1 kHz rate + + wait(0.1); // Add delay for accumulation of samples + + c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x07); // Clear high-pass filter bits [2:0] + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | 0x07); // Set ACCEL_HPF to 7; hold the initial accleration value as a referance + + c = readByte(MPU6050_ADDRESS, PWR_MGMT_2); + writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c & ~0xC7); // Clear standby XA, YA, and ZA bits [3:5] and LP_WAKE_CTRL bits [6:7] + writeByte(MPU6050_ADDRESS, PWR_MGMT_2, c | 0x47); // Set wakeup frequency to 5 Hz, and disable XG, YG, and ZG gyros (bits [0:2]) + + c = readByte(MPU6050_ADDRESS, PWR_MGMT_1); + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c & ~0x20); // Clear sleep and cycle bit 5 + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, c | 0x20); // Set cycle bit 5 to begin low power accelerometer motion interrupts + +} + + +void resetMPU6050() { + // reset device + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device + wait(0.1); + } + + +void initMPU6050() +{ + // Initialize MPU6050 device + // wake up device + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors + wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt + + // get stable time source + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 + + // Configure Gyro and Accelerometer + // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively; + // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both + // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate + writeByte(MPU6050_ADDRESS, CONFIG, 0x03); + + // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) + writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above + + // Set gyroscope full scale range + // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 + uint8_t c = readByte(MPU6050_ADDRESS, GYRO_CONFIG); + writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] + writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3] + writeByte(MPU6050_ADDRESS, GYRO_CONFIG, c | Gscale << 0); // Set full scale range for the gyro + + // Set accelerometer configuration + c = readByte(MPU6050_ADDRESS, ACCEL_CONFIG); + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3] + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, c | Ascale << 0); // Set full scale range for the accelerometer + + // Configure Interrupts and Bypass Enable + // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips + // can join the I2C bus and all can be controlled by the Arduino as master + writeByte(MPU6050_ADDRESS, INT_PIN_CFG, 0x22); + writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt +} + +// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average +// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers. +void calibrateMPU6050(float * dest1, float * dest2) +{ + uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data + uint16_t ii, packet_count, fifo_count; + int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; + +// reset device, reset all registers, clear gyro and accelerometer bias registers + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device + wait_ms(100); + +// get stable time source +// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001 + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x01); + writeByte(MPU6050_ADDRESS, PWR_MGMT_2, 0x00); + wait_ms(200); + +// Configure device for bias calculation + writeByte(MPU6050_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts + writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable FIFO + writeByte(MPU6050_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source + writeByte(MPU6050_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master + writeByte(MPU6050_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes + writeByte(MPU6050_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP + wait_ms(20); + +// Configure MPU6050 gyro and accelerometer for bias calculation + writeByte(MPU6050_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz + writeByte(MPU6050_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz + writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity + + uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec + uint16_t accelsensitivity = 16384; // = 16384 LSB/g + +// Configure FIFO to capture accelerometer and gyro data for bias calculation + writeByte(MPU6050_ADDRESS, USER_CTRL, 0x40); // Enable FIFO + writeByte(MPU6050_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 1024 bytes in MPU-6050) + wait_us(78250); // accumulate 80 samples in 80 milliseconds = 960 bytes + +// At end of sample accumulation, turn off FIFO sensor read + writeByte(MPU6050_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO + readBytes(MPU6050_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count + fifo_count = ((uint16_t)data[0] << 8) | data[1]; + packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging + // pc.printf("packet = %d", packet_count); + + for (ii = 0; ii < packet_count; ii++) { + int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; + readBytes(MPU6050_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging + accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO + accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; + accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; + gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; + gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; + gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; + + accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + accel_bias[1] += (int32_t) accel_temp[1]; + accel_bias[2] += (int32_t) accel_temp[2]; + gyro_bias[0] += (int32_t) gyro_temp[0]; + gyro_bias[1] += (int32_t) gyro_temp[1]; + gyro_bias[2] += (int32_t) gyro_temp[2]; + +} + +pc.printf("packet = %d, filo = %d", packet_count,fifo_count); + accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases + accel_bias[1] /= (int32_t) packet_count; + accel_bias[2] /= (int32_t) packet_count; + gyro_bias[0] /= (int32_t) packet_count; + gyro_bias[1] /= (int32_t) packet_count; + gyro_bias[2] /= (int32_t) packet_count; + + if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation + else {accel_bias[2] += (int32_t) accelsensitivity;} + +// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup + data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format + data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases + data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; + data[3] = (-gyro_bias[1]/4) & 0xFF; + data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; + data[5] = (-gyro_bias[2]/4) & 0xFF; + +// Push gyro biases to hardware registers + writeByte(MPU6050_ADDRESS, XG_OFFS_USRH, data[0]); + writeByte(MPU6050_ADDRESS, XG_OFFS_USRL, data[1]); + writeByte(MPU6050_ADDRESS, YG_OFFS_USRH, data[2]); + writeByte(MPU6050_ADDRESS, YG_OFFS_USRL, data[3]); + writeByte(MPU6050_ADDRESS, ZG_OFFS_USRH, data[4]); + writeByte(MPU6050_ADDRESS, ZG_OFFS_USRL, data[5]); + + dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction + dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; + dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; + +// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain +// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold +// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature +// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that +// the accelerometer biases calculated above must be divided by 8. + + int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases + readBytes(MPU6050_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values + accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1]; + readBytes(MPU6050_ADDRESS, YA_OFFSET_H, 2, &data[0]); + accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1]; + readBytes(MPU6050_ADDRESS, ZA_OFFSET_H, 2, &data[0]); + accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1]; + + uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers + uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis + + for(ii = 0; ii < 3; ii++) { + if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit + } + + // Construct total accelerometer bias, including calculated average accelerometer bias from above + accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) + accel_bias_reg[1] -= (accel_bias[1]/8); + accel_bias_reg[2] -= (accel_bias[2]/8); + + data[0] = (accel_bias_reg[0] >> 8) & 0xFF; + data[1] = (accel_bias_reg[0]) & 0xFF; + data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[2] = (accel_bias_reg[1] >> 8) & 0xFF; + data[3] = (accel_bias_reg[1]) & 0xFF; + data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[4] = (accel_bias_reg[2] >> 8) & 0xFF; + data[5] = (accel_bias_reg[2]) & 0xFF; + data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers + + // Push accelerometer biases to hardware registers +// writeByte(MPU6050_ADDRESS, XA_OFFSET_H, data[0]); +// writeByte(MPU6050_ADDRESS, XA_OFFSET_L_TC, data[1]); +// writeByte(MPU6050_ADDRESS, YA_OFFSET_H, data[2]); +// writeByte(MPU6050_ADDRESS, YA_OFFSET_L_TC, data[3]); +// writeByte(MPU6050_ADDRESS, ZA_OFFSET_H, data[4]); +// writeByte(MPU6050_ADDRESS, ZA_OFFSET_L_TC, data[5]); + +// Output scaled accelerometer biases for manual subtraction in the main program + dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; + dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; + dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; +printf("set acc : x= %f\t,y= %f\tz= %f\r\n;",accelBias[0],accelBias[1],accelBias[2]); + +} + + +// Accelerometer and gyroscope self test; check calibration wrt factory settings +void MPU6050SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass +{ + uint8_t rawData[4] = {0, 0, 0, 0}; + uint8_t selfTest[6]; + float factoryTrim[6]; + + // Configure the accelerometer for self-test + writeByte(MPU6050_ADDRESS, ACCEL_CONFIG, 0xF0); // Enable self test on all three axes and set accelerometer range to +/- 8 g + writeByte(MPU6050_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s + wait(0.25); // Delay a while to let the device execute the self-test + rawData[0] = readByte(MPU6050_ADDRESS, SELF_TEST_X); // X-axis self-test results + rawData[1] = readByte(MPU6050_ADDRESS, SELF_TEST_Y); // Y-axis self-test results + rawData[2] = readByte(MPU6050_ADDRESS, SELF_TEST_Z); // Z-axis self-test results + rawData[3] = readByte(MPU6050_ADDRESS, SELF_TEST_A); // Mixed-axis self-test results + // Extract the acceleration test results first + selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4 ; // XA_TEST result is a five-bit unsigned integer + selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4 ; // YA_TEST result is a five-bit unsigned integer + selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) >> 4 ; // ZA_TEST result is a five-bit unsigned integer + // Extract the gyration test results first + selfTest[3] = rawData[0] & 0x1F ; // XG_TEST result is a five-bit unsigned integer + selfTest[4] = rawData[1] & 0x1F ; // YG_TEST result is a five-bit unsigned integer + selfTest[5] = rawData[2] & 0x1F ; // ZG_TEST result is a five-bit unsigned integer + // Process results to allow final comparison with factory set values + factoryTrim[0] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[0] - 1.0f)/30.0f))); // FT[Xa] factory trim calculation + factoryTrim[1] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[1] - 1.0f)/30.0f))); // FT[Ya] factory trim calculation + factoryTrim[2] = (4096.0f*0.34f)*(pow( (0.92f/0.34f) , ((selfTest[2] - 1.0f)/30.0f))); // FT[Za] factory trim calculation + factoryTrim[3] = ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[3] - 1.0f) )); // FT[Xg] factory trim calculation + factoryTrim[4] = (-25.0f*131.0f)*(pow( 1.046f , (selfTest[4] - 1.0f) )); // FT[Yg] factory trim calculation + factoryTrim[5] = ( 25.0f*131.0f)*(pow( 1.046f , (selfTest[5] - 1.0f) )); // FT[Zg] factory trim calculation + + // Output self-test results and factory trim calculation if desired + // Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]); + // Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]); + // Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]); + // Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]); + + // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response + // To get to percent, must multiply by 100 and subtract result from 100 + for (int i = 0; i < 6; i++) { + destination[i] = 100.0f + 100.0f*(selfTest[i] - factoryTrim[i])/factoryTrim[i]; // Report percent differences + } + +} + + +// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays" +// (see http://www.x-io.co.uk/category/open-source/ for examples and more details) +// which fuses acceleration and rotation rate to produce a quaternion-based estimate of relative +// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. +// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms +// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! + void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz) + { + float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability + float norm; // vector norm + float f1, f2, f3; // objective funcyion elements + float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33; // objective function Jacobian elements + float qDot1, qDot2, qDot3, qDot4; + float hatDot1, hatDot2, hatDot3, hatDot4; + float gerrx, gerry, gerrz, gbiasx, gbiasy, gbiasz; // gyro bias error + + // Auxiliary variables to avoid repeated arithmetic + float _halfq1 = 0.5f * q1; + float _halfq2 = 0.5f * q2; + float _halfq3 = 0.5f * q3; + float _halfq4 = 0.5f * q4; + float _2q1 = 2.0f * q1; + float _2q2 = 2.0f * q2; + float _2q3 = 2.0f * q3; + float _2q4 = 2.0f * q4; +// float _2q1q3 = 2.0f * q1 * q3; +// float _2q3q4 = 2.0f * q3 * q4; + + // Normalise accelerometer measurement + norm = sqrt(ax * ax + ay * ay + az * az); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + ax *= norm; + ay *= norm; + az *= norm; + + // Compute the objective function and Jacobian + f1 = _2q2 * q4 - _2q1 * q3 - ax; + f2 = _2q1 * q2 + _2q3 * q4 - ay; + f3 = 1.0f - _2q2 * q2 - _2q3 * q3 - az; + J_11or24 = _2q3; + J_12or23 = _2q4; + J_13or22 = _2q1; + J_14or21 = _2q2; + J_32 = 2.0f * J_14or21; + J_33 = 2.0f * J_11or24; + + // Compute the gradient (matrix multiplication) + hatDot1 = J_14or21 * f2 - J_11or24 * f1; + hatDot2 = J_12or23 * f1 + J_13or22 * f2 - J_32 * f3; + hatDot3 = J_12or23 * f2 - J_33 *f3 - J_13or22 * f1; + hatDot4 = J_14or21 * f1 + J_11or24 * f2; + + // Normalize the gradient + norm = sqrt(hatDot1 * hatDot1 + hatDot2 * hatDot2 + hatDot3 * hatDot3 + hatDot4 * hatDot4); + hatDot1 /= norm; + hatDot2 /= norm; + hatDot3 /= norm; + hatDot4 /= norm; + + // Compute estimated gyroscope biases + gerrx = _2q1 * hatDot2 - _2q2 * hatDot1 - _2q3 * hatDot4 + _2q4 * hatDot3; + gerry = _2q1 * hatDot3 + _2q2 * hatDot4 - _2q3 * hatDot1 - _2q4 * hatDot2; + gerrz = _2q1 * hatDot4 - _2q2 * hatDot3 + _2q3 * hatDot2 - _2q4 * hatDot1; + + // Compute and remove gyroscope biases + gbiasx += gerrx * deltat * zeta; + gbiasy += gerry * deltat * zeta; + gbiasz += gerrz * deltat * zeta; + // gx -= gbiasx; + // gy -= gbiasy; + // gz -= gbiasz; + + // Compute the quaternion derivative + qDot1 = -_halfq2 * gx - _halfq3 * gy - _halfq4 * gz; + qDot2 = _halfq1 * gx + _halfq3 * gz - _halfq4 * gy; + qDot3 = _halfq1 * gy - _halfq2 * gz + _halfq4 * gx; + qDot4 = _halfq1 * gz + _halfq2 * gy - _halfq3 * gx; + + // Compute then integrate estimated quaternion derivative + q1 += (qDot1 -(beta * hatDot1)) * deltat; + q2 += (qDot2 -(beta * hatDot2)) * deltat; + q3 += (qDot3 -(beta * hatDot3)) * deltat; + q4 += (qDot4 -(beta * hatDot4)) * deltat; + + // Normalize the quaternion + norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion + norm = 1.0f/norm; + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; + + } + + void calibrate_manual_MPU6050(float * dest1, float * dest2){ + //giroscopio + dest1[0] = 0;//(float) gyro_bias[0]/(float) gyrosensitivity; + dest1[1] =0;// ((float) gyro_bias[1]/(float) gyrosensitivity; + dest1[2] = 0;//(float) gyro_bias[2]/(float) gyrosensitivity; + + //acceletomtro + dest2[0] = 0;//(float)accel_bias[0]/(float)accelsensitivity; + dest2[1] = 0;//float)accel_bias[1]/(float)accelsensitivity; + dest2[2] = 0;//(float)accel_bias[2]/(float)accelsensitivity; + + + } + + + }; +#endif \ No newline at end of file
diff -r 000000000000 -r a9753886e1e0 main_imu.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main_imu.h Sun Nov 05 14:20:26 2017 +0000 @@ -0,0 +1,175 @@ +/* MPU6050 Basic Example Code + by: Kris Winer + date: May 1, 2014 + license: Beerware - Use this code however you'd like. If you + find it useful you can buy me a beer some time. + + Demonstrate MPU-6050 basic functionality including initialization, accelerometer trimming, sleep mode functionality as well as + parameterizing the register addresses. Added display functions to allow display to on breadboard monitor. + No DMP use. We just want to get out the accelerations, temperature, and gyro readings. + + SDA and SCL should have external pull-up resistors (to 3.3V). + 10k resistors worked for me. They should be on the breakout + board. + + Hardware setup: + MPU6050 Breakout --------- Arduino + 3.3V --------------------- 3.3V + SDA ----------------------- A4 + SCL ----------------------- A5 + GND ---------------------- GND + + Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library. + Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1. + We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file. + We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. + */ + #include "MPU6050.h" +void calcola_dati(); +float sum = 0; +uint32_t sumCount = 0; + +MPU6050 mpu6050; +Timer t; + +Thread calcolo_q; + +//void pc_trasmisione(int n,char* s); +bool inPosition=true; + + +#include "racolta_dati.h" + char buffer[100]; + + +void main_imu() // prendere tutto questo main e meterno in main_imu, rinominarlo e aviorlo da qui. +{ + // char n; pacco posta; + + using namespace mydati; + + dati_imu myimu; + + + + + + + //Set up I2C + i2c.frequency(100000); // use fast (400 kHz) I2C + t.start(); + + + + + wait_ms(350); + + inPosition=pul;//vero se non è premuto + if(inPosition)pc.printf("pulsante premuto!"); + + // Read the WHO_AM_I register, this is a good test of communication + uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 + pc.printf("\t\tI AM 0x%x\n\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); + + + if (whoami == 0x68) // WHO_AM_I should always be 0x68 + { + pc.printf("MPU6050 is online..."); + wait_ms(50); + + + + mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values + pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%d", SelfTest[0]); pc.printf("% of factory value \n\r"); + pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%d", SelfTest[1]); pc.printf("% of factory value \n\r"); + pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%d", SelfTest[2]); pc.printf("% of factory value \n\r"); + pc.printf("x-axis self test: gyration trim within : "); pc.printf("%d", SelfTest[3]); pc.printf("% of factory value \n\r"); + pc.printf("y-axis self test: gyration trim within : "); pc.printf("%d", SelfTest[4]); pc.printf("% of factory value \n\r"); + pc.printf("z-axis self test: gyration trim within : "); pc.printf("%d", SelfTest[5]); pc.printf("% of factory value \n\r"); + wait(1); + + if(inPosition && SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) + { + mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration + mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers + mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature + + + wait(2); + pc.printf("set acc : x= %f\t,y= %f\tz= %f\r\n;",accelBias[0],accelBias[1],accelBias[2]); + } + else + { + pc.printf("Device did not the pass self-test!\n\r"); + + + } + } + else + { + pc.printf("Could not connect to MPU6050: \n\n\r"); + pc.printf("%#x \n", whoami); + + + + while(1) ; // Loop forever if communication doesn't happen + } + + + + calcolo_q.start(calcola_dati); + + while(1) { + + wait_ms(100); + + sprintf(buffer,"\tax = %6.1f\tay = %6.1f\taz = %6.1f\t\t", 1000*ax,1000*ay,1000*az); + //sprintf(buffer,"ciao"); + wait_ms(10); + + #if test + + + //sprintf(buffer,"\tax = %6.1f\tay = %6.1f\taz = %6.1f mg\t\t", 1000*ax,100*ay,100*az); + + + pc.printf("\tax = %6.1f", 1000*ax); + pc.printf(" ay = %6.1f", 1000*ay); + pc.printf(" az = %6.1f mg\t\t", 1000*az); + + pc.printf("gx = %6.1f", gx); + pc.printf(" gy = %6.1f", gy); + pc.printf(" gz = %6.1f deg/s\t", gz); + // pc.printf("Yaw: %.2f , Pitch: %.2f, Roll: %.2f", yaw, pitch, roll); + pc.printf("\t temperature = %.2f C\n\r", temperature); + // pc.printf("q0 = %f\tq1 = %f\tq2 = %f\tq3 = %f\n\r", q[0],q[1],q[2],q[3]); + + n=strlen(buffer); + posta.n=n+1; + + posta.txt=buffer; + + // telemetria.ins_in_coda(&posta); + + wait_ms(1); + // telemetria.invio(); + wait_ms(1); + + // pc.printf("q0 = %f\tq1 = %f\tq2 = %f\tq3 = %f\n\r", q[0],q[1],q[2],q[3]); + + #endif + + + myimu.set_all(ax,ay,az,gx,gy,gz,0,0,0,temperature); + wait_ms(2); + myimu.invia(); + wait_ms(2); + //da sostituire con la funzione della classe sensore imu + + + + //myled= !myled; +} + +} +
diff -r 000000000000 -r a9753886e1e0 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Nov 05 14:20:26 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/fb8e0ae1cceb \ No newline at end of file
diff -r 000000000000 -r a9753886e1e0 my_img_sd.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/my_img_sd.h Sun Nov 05 14:20:26 2017 +0000 @@ -0,0 +1,215 @@ +#ifndef img_sd_h +#define img_sd_h + + + +#define BLOCK_START_ADDR 0 /* Block start address */ + +#define BLOCKSIZE 512 /* Block Size in Bytes */ +#define NUM_OF_BLOCKS 2 /* Total number of blocks */ +#define BUFFER_WORDS_SIZE ((BLOCKSIZE * NUM_OF_BLOCKS) >> 2) /* Total data size in bytes */ + +#include <mbed.h> + +Serial pc(USBTX, USBRX); // tx, rx + +void stampa_B(uint32_t b[],int n,int x,int y,int x0,int y0,int x_0,int y_0); +void riemp_B(uint32_t b[],int n,int x0,int y0,int x,int y); + + +void serializza_img(uint32_t** img ,int x,int y,uint64_t addr){ + + int n_b=4*x*y/BLOCKSIZE +1; + uint32_t Buffer[BLOCKSIZE]; + Buffer[0]=n_b; + Buffer[1]=0; + Buffer[2]=x; + Buffer[3]=y; + // pc.printf("n_b= %d\r\n",n_b); + riemp_B(&Buffer[4],124,0,0,x,y); + if(sd.WriteBlocks(Buffer,addr, BLOCKSIZE, 1) == SD_OK) + pc.printf("SD OK"); + int incx,incy; + incx=128/y; + incy=128%y; + int x0,y0; + x0=124/y; + y0=124%y; + for(int i=1;i<n_b;i++){ + // pc.printf("sto caricando il buffer.. "); + riemp_B(Buffer,128,x0,y0,x,y); + if(sd.WriteBlocks(Buffer,addr+BLOCKSIZE*i, BLOCKSIZE,1) != SD_OK) + pc.printf("Errore i= %d",i); + x0+=incx; + y0+=incy; + if(y0>=y){ + x0++; + y0-=y; + } + + } + + //pc.printf("ok, serializato!\r\n"); + + + +} +unsigned int N_bk = 120; + +void leggi_img(int x_0,int y_0,uint64_t addr){ + int x,y; + uint32_t Buffer[N_bk*BLOCKSIZE+1]; + int n_b=0; + if(sd.ReadBlocks(Buffer, addr, BLOCKSIZE,1)== SD_OK){ + // pc.printf("SD READ : OK.\r\n"); + + n_b=Buffer[0]; + x=Buffer[2]; + y=Buffer[3]; + // pc.printf("nb= %d\r\n",n_b); + // pc.printf("x = %d - y= %d\r\n",x,y); + } + if(n_b<N_bk)N_bk=n_b; + stampa_B(&Buffer[4],124,x,y,0,0,x_0,y_0); + int incx,incy; + incx=N_bk*128/y; + incy=(N_bk*128)%y; + int x0,y0; + x0=124/y; + y0=124%y; + + + // pc.printf("x= %d, y= %d, x0= %d, yo= %d, y_00= %d\r\n",x,y,x0,y0,y_0); + for(int i=1;i<n_b;i+=N_bk) + + if(sd.ReadBlocks(Buffer, addr+BLOCKSIZE*i, BLOCKSIZE,N_bk)== SD_OK){ + // pc.printf("SD READ %d: OK.\r\n",i); + stampa_B(Buffer,128*N_bk,x,y,x0,y0,x_0,y_0); + x0+=incx; + y0+=incy; + if(y0>=y){ + x0++; + y0-=y; + } + + } + + + // pc.printf("finito !"); + +} + + +void riemp_B(uint32_t b[],int n,int x0,int y0,int x,int y){ + + // pc.printf("ok\r\n"); +} + +void stampa_B(uint32_t b[],int n,int x,int y,int x0,int y0,int x_0,int y_0){ + int cont=0; + int y1=y0; + int j; + + for(int i=x0;i<x;i++) + for(j=y1;j<y;j++){ + y1=0; + lcd.DrawPixel(j+x_0,x-i+y_0,b[(i-x0)*y+j-y0]); + //cont++; + if(++cont==n)return; + } +} + +void scrivi_dato(uint32_t Buffer[],uint64_t addr,int n=1){ + //pc.baud(921600); +if(addr>=start_addr_dato){ + if(sd.WriteBlocks(Buffer,addr, BLOCKSIZE*n,1) != SD_OK){ + pc.printf("\t\t\t\t -> Errore scrittura : "); + pc.printf("addres = %x \r\n",addr); + } + else + { + // pc.printf("non salvato:\t%s\r\n",(char*)Buffer); + // pc.printf("addres = %x ",addr); + + //slave.address(0xA0); + + } + } + else pc.printf("errore indirizzo non valido, addr= %d -- addr_Base = %d\r\n",addr,start_addr_dato); +} + +void printa_dato(uint32_t Buffer[],uint64_t addr){ +if(addr>=start_addr_dato) + if(sd.ReadBlocks(Buffer,addr, BLOCKSIZE,1) != SD_OK) + pc.printf("errore lettura"); + else pc.printf("Dato letto: %s",(char*)Buffer); +} + +void scrivi_dato_progresione(uint32_t Buffer[]){ + /* + uint32_t B[512]; + sd.ReadBlocks((uint32_t*)B,0, BLOCKSIZE,1); + B[10]++; + sd.WriteBlocks((uint32_t*)B,0, BLOCKSIZE,1); + + scrivi_dato(Buffer,512*B[10]); + */ + static int u=0; + + static int n=-1; + const int k=32; + static uint32_t Dato_B[BLOCKSIZE*k]; + static int B[BLOCKSIZE]; + uint64_t addr; + //pc.printf("n=%d",n); + if(n<0){pc.printf("inizio scritura\r\n");sd.ReadBlocks((uint32_t*)B,0, BLOCKSIZE,1);n=0; + if(B[10]<start_addr_dato/512)pc.printf("problemone, l' indirizzo salvato non è valido!\r\n"); + } + //pc.printf("inizio scritura"); + + if(n==k-1){ + + //sd.ReadBlocks((uint32_t*)B,0, BLOCKSIZE,1); + sd.ReadBlocks((uint32_t*)B,0, BLOCKSIZE,1); + strcpy((char*)Dato_B[BLOCKSIZE*n],(char*)Buffer); + addr=B[10]; + addr=BLOCKSIZE*addr; + + scrivi_dato(Dato_B,addr,k); + + u++; + //if(u==640||u==800||u==2240||u==2976||u==2720)B[10]=B[10]+2*k;else + B[10]=B[10]+k; + + sd.WriteBlocks((uint32_t*)B,0, BLOCKSIZE,1); + + n=0; + } + else {strcpy((char*)Dato_B[BLOCKSIZE*n],(char*)Buffer);n++;} + + + +} + +void printa_dato_progressione(){ + + pc.printf("\r\n Sto per legere in progresione ... "); + uint32_t B[BLOCKSIZE]; + sd.ReadBlocks((uint32_t*)B,0, BLOCKSIZE,1); + int n=B[10]; + pc.printf("n=%d\r\n",n-start_addr_dato/512); + for(int i=start_addr_dato;i<n*BLOCKSIZE;i+=BLOCKSIZE) + printa_dato((uint32_t*)B,i); + + pc.printf("\tfine\r\n"); +} + +void cancella_dati(){ + + int B[512]; + pc.printf("\n dati cancellati \n"); + B[10]=start_addr_dato/512; + sd.WriteBlocks((uint32_t*)B,0, BLOCKSIZE,1); +} + +#endif \ No newline at end of file
diff -r 000000000000 -r a9753886e1e0 racolta_dati.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/racolta_dati.h Sun Nov 05 14:20:26 2017 +0000 @@ -0,0 +1,104 @@ + +void calcola_dati(){ + while(true){ + wait_us(20); + // If data ready bit set, all data registers have new data + if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt + mpu6050.readAccelData(accelCount); // Read the x/y/z adc values + mpu6050.getAres(); + + // Now we'll calculate the accleration value into actual g's + ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set + ay = (float)accelCount[1]*aRes - accelBias[1]; + az = (float)accelCount[2]*aRes - accelBias[2]; + + mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values + mpu6050.getGres(); + + // Calculate the gyro value into actual degrees per second + gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set + gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; + gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; + + tempCount = mpu6050.readTempData(); // Read the x/y/z adc values + temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade + } + + Now = t.read_us(); + deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update + lastUpdate = Now; + + sum += deltat; + sumCount++; + + if(lastUpdate - firstUpdate > 10000000.0f) { + beta = 0.04; // decrease filter gain after stabilized + zeta = 0.015; // increasey bias drift gain after stabilized + } + + // Pass gyro rate as rad/s + gx=(int)gx;gy=(int)gy;gz=(int)gz; + + ax=((int)10000*ax)/10000; + ay=((int)10000*ay)/10000; + az=((int)10000*az)/10000; + + q[0]=((int)10000*q[0])/10000; + q[1]=((int)10000*q[1])/10000; + q[2]=((int)10000*q[2])/10000; + q[3]=((int)10000*q[3])/10000; + + mpu6050.MadgwickQuaternionUpdate(ax, ay,az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); + + // Serial print and/or display at 0.5 s rate independent of data rates + delt_t = t.read_ms() - count; + if (delt_t > 500) { // update LCD once per half-second independent of read rate +#if false + pc.printf("\tax = %6.1f", 1000*ax); + pc.printf(" ay = %6.1f", 1000*ay); + pc.printf(" az = %6.1f mg\t\t", 1000*az); + + pc.printf("gx = %6.1f", gx); + pc.printf(" gy = %6.1f", gy); + pc.printf(" gz = %6.1f deg/s\t\t\t", gz); + pc.printf("\t\t temperature = %f C\n\r", temperature); + + + // pc.printf("q0 = %f\tq1 = %f\tq2 = %f\tq3 = %f\n\r", q[0],q[1],q[2],q[3]); + #endif + // pc.printf("q1 = %f\n\r", q[1]); pc.printf("q2 = %f\n\r", q[2]); pc.printf("q3 = %f\n\r", q[3]); + + + + + + // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. + // In this coordinate system, the positive z-axis is down toward Earth. + // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. + // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. + // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. + // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. + // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be + // applied in the correct order which for this configuration is yaw, pitch, and then roll. + // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. + + //sbagliato -> da fare tutto da capo. usare solo l' accelerometro per pich e rol, lo yaw non serve. + + yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]),2.0f *(q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3])); //<--- quel coglione ha sbagliato a scrive l' equazione con i quaternioni + pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); + roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]),2.0f* (q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3])); + pitch *= 180.0f / PI; + yaw *= 180.0f / PI; + roll *= 180.0f / PI; + + + // pc.printf("Yaw, Pitch, Roll: %.2f %.2f %.2f", yaw, pitch, roll); + // pc.printf("\taverage rate = %f\n\r", (float) sumCount/sum); + + + count = t.read_ms(); + sum = 0; + sumCount = 0; + } + } +} \ No newline at end of file
diff -r 000000000000 -r a9753886e1e0 trasmetti.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trasmetti.cpp Sun Nov 05 14:20:26 2017 +0000 @@ -0,0 +1,185 @@ +#define tra_cpp +#include "trasmetti.h" +#include <cstring> +//#include <iostream> + +namespace mydati{ + + + +bool trasmetti::ins_in_testa(pacco* d,char priorita){ + coda* app; + app =new coda; + if(!app)return false; + app->dato=d; + app->priorita=priorita; + n++; + app->suc=testa; + testa=app; + delete [] d->txt; + return true; + +} +bool trasmetti::ins_in_coda(pacco* d,char priorita){ + + coda* app; + app =new coda; + if(!app)return false; + // pc.printf("funzione ins coda 1 \r\n"); + + app->dato=d; + + /* + app->dato=new pacco; + app->dato->n=d->n; + app->dato->txt=new char[d->n]; + strcpy(app->dato->txt,d->txt); + delete [] d->txt; + */ + + if(!n){testa=fine=app; n++;return true;} +//pc.printf("funzione ins coda 3 \r\n"); + fine->suc=app; + fine=app; + fine->suc=0; + n++; + return true; +}; +bool trasmetti::ins_priorita(pacco* d,char priorita){ + + coda* app; + app =new coda; + if(!app)return false; + app->dato=d; + + app->priorita=priorita; + + if(!n){app->suc=0;testa=fine=app; n++;return true;} + if(testa->priorita<priorita){n++;app->suc=testa; testa=app;return true;} + + coda* cerca = testa; + if(n!=1){ bool tr =true; + while(cerca->suc->suc!=0&&tr) + if(cerca->suc->priorita>=priorita) + { + cerca=cerca->suc; + } + else tr=false; + + app->suc=cerca->suc; + cerca->suc=app; + n++; + return true; + } + fine->suc=app; + fine=app; + fine->suc=0; + n++; + return true; +}; + +bool trasmetti::invio(){ // iovio i dati! + + if(!n)return false; + pacco* app; + extract(&app); + +if(!pc_trasmisione(app->n,app->txt))return false; + delete [] app->txt; + delete app; + + return true; +} +void trasmetti::extract(pacco** d){ + if(n!=0){ + + coda* app=testa; + *d=testa->dato; + testa=testa->suc; + n--; + + delete app; + } + +} +bool dati_imu::invia(char p){ + //da agiungere una stringa per il protocolla dei tipo dei dati... + //es #nomeclasse@dati + pacco* app; + app= new pacco; + app->n=nb_classe; + app->txt=new char[nb_classe+1]; + sprintf(app->txt,"A%03.0f%03.0f%03.0f%04.0f%04.0f%04.0f#%d",1000*ax+off_set_a,1000*ay+off_set_a,1000*az+off_set_a,10*gx+off_set_g,10*gy+off_set_g,10*gz+off_set_g,number()); + + + //app->txt=buffer; + /* + app->txt=new char[strlen(buffer)+1]; + //printf("ximu %d X \n",strlen(buffer)); + strcpy(app->txt,buffer); + */ + //*funzione se si vuole usare la trasmisione binaria + //memcpy((void*)app->txt,(void*)this,nb_classe); + + + + return telemetria.ins_in_coda(app,p); +} + + + + +bool estensimetro::invia(char p){ + pacco* app; + app= new pacco; + app->n=nb_classe; + app->txt=new char[nb_classe+1]; + sprintf(app->txt,"B%04.0f%04.0f%04.0f%04.0f#%d",ad,as,pd,ps,number()); + + + + return telemetria.ins_in_coda(app,p); +} + +bool ruota_fonica::invia(char p){ + pacco* app; + app= new pacco; + app->n=nb_classe; + app->txt=new char[nb_classe+1]; + + sprintf(app->txt,"C%04.0f%04.0f%04.0f%04.0f#%d",ad,as,pd,ps,number()); + //sprintf(app->txt,"C%04.0f%04.0f#%d",ad,as,pd,ps,number()); + + + return telemetria.ins_in_coda(app,p); +} + +bool motore::invia(char p){ + pacco* app; + app= new pacco; + app->n=nb_classe; + //char buffer[50]; + app->txt=new char[nb_classe+1]; + + + if(P_olio>10){pc.printf("errore olio troppo alto");P_olio=9.9;} + + if(velocita>10000){pc.printf("errore velocita troppo alta!");velocita=9999;} + //D 2000 7700 80800 9400 22 5#2 + sprintf(app->txt,"D%03d%03d%05.0f%04.0f%02.0f%d#%d",T_acqua,T_olio,RMP,velocita,10*P_olio,marcia,number()); + // pc.printf("funzione motore.invia \r\n"); + return telemetria.ins_in_coda(app,p); +} +bool allert::invia(const char* s){ + + + pacco* app; + app= new pacco; + app->n=strlen(s+1); + //app->txt=new char[app->n+1]; + app->txt=(char*)s; + + return telemetria.ins_in_testa(app); + } +} +
diff -r 000000000000 -r a9753886e1e0 trasmetti.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/trasmetti.h Sun Nov 05 14:20:26 2017 +0000 @@ -0,0 +1,225 @@ +//#ifndef mydati_h +//#odefine mydati_h +#include "mbed.h" + + + +Serial pc(USBTX, USBRX); // tx, rx + +#ifndef mydati_h +#define mydati_h +extern Serial pc; +extern Serial device; +extern Serial device2; + +namespace mydati{ + +bool pc_trasmisione(int,char* s); + + struct pacco{ + char* txt; + char n; + }; + +class trasmetti{ /* classe superiore che ha il compito di interagire con l' altra scheda, + le altre sottoclassi si intrfacciano a lei */ +private: + int addr_slave; + I2C* master; + char conteggio; + + struct coda{ + char priorita; + pacco* dato; + coda* suc; + }; + coda* testa; + coda* fine; + char n; + void extract(pacco**); +public: +trasmetti():n(0){testa=fine=0;} + +void set_add(int a){addr_slave=a;} +void set_master(I2C* a){master=a;} + +bool ins_in_coda(pacco* ,char priorita= 0); +bool ins_priorita(pacco* ,char priorita= 0); +bool ins_in_testa(pacco* ,char priorita= 0); + +bool pc_trasmisione(int n,char* s); + +bool invio(); +} +//serve perche deve essere unina per tutti e quindi di variabile di qeusta classe ne deve essere creata solo una +#ifdef tra_cpp +telemetria; +#else +; +extern trasmetti telemetria; +#endif +class sensore{ //classe generia appezotata, da riempire sucesivamente per oblicare alle altri figlie di implemetare la funzione + private: + static const int max_n=10; + int n; + + public: + sensore():n(-1){} + virtual bool invia(char p=0)=0; + int number(){n++;n=n%max_n;return n;} + virtual ~sensore(){} +}; + + +/*le sucesive classi hanno il compito di legere i dati dal sensore +e di inviari interfaciandosi con la classe di sopra, +le funzioni che prendoi i dati vengono cambiate, c'è un set_all che viene implementato in mbed, +l' altro serve a me per testare il tutto con il cp +*/ +class dati_imu : public sensore{ +private: +static const char priorita_imu =5; +static const int nb_classe =25; + float ax,ay,az; + float gx,gy,gz; + float pich,roll,yaw; + float temp; + static const int off_set_a=2000; + static const int off_set_g=1000; +public: +//dati_imu():sensore(),ax(0),ay(0),az(0),gx(0),gy(0),gz(0),pich(0),roll(0),yaw(0),temp(0){} + +void set_all(float a,float b,float c,float d,float e,float f,float g,float h, float i, float t){ // da elimiare i parametri di ingresso, essa chiamera una funzione dedicata + ax=a;ay=b;az=c;gx=d;gy=e;gz=f;pich=g;roll=h;yaw=i;temp=t; + } +void set_all(); + +virtual bool invia(char p=priorita_imu); +}; + +class estensimetro : public sensore{ + private: + static const char priorita_estensimetro =4; + static const int nb_classe =17; + float ad,as,pd,ps; // a= anteriore p= posteriore + public: + void set_all(float vad,float vas,float vpd,float vps){ad=vad; as=vas; pd=vpd; ps=vps;} + void set_all(); + virtual bool invia(char p=priorita_estensimetro); +}; + +class ruota_fonica : public sensore{ + private: + static const char priorita_ruota_fonica =4; + static const int nb_classe =17; + float ad,as,pd,ps; // a= anteriore p= posteriore + public: + void set_all(float vad,float vas,float vpd,float vps){ad=vad; as=vas; pd=vpd; ps=vps;} + void set_all(){} + virtual bool invia(char p=priorita_ruota_fonica); +}; + +class motore : public sensore{ + private: + static const char priorita_motore =4; + static const int nb_classe =24; + float P_olio,RMP,velocita; + char marcia,T_acqua,T_olio; + public: + motore():P_olio(0),RMP(0),velocita(0),marcia(0),T_acqua(0),T_olio(0){} + void set_all(char Ta,char To,float Po,float vrmp,float vel, char m){T_acqua=Ta; T_olio=To;P_olio=Po;RMP=vrmp;velocita=vel;marcia=m;} + void set_1(float a,char b){RMP=a;T_acqua=b;} + void set_velocita_gear(float a,char b){velocita=a;marcia=b;} + void set_T_oil(char a){T_olio=a;} + void set_P_oil(float a){P_olio=a;} + + char get_marcia()const{return marcia;} + char get_Tacq()const{return T_acqua;} + char get_Tolio()const{return T_olio;} + float get_Polio()const{return P_olio;} + float get_rmp()const{return RMP;} + float get_vel()const{return velocita;} + + virtual bool invia(char p=priorita_motore); +}; + +class allert{ + public: + bool invia(const char*); +}; +#ifdef tra_cpp + +void led_rpm(int rpm, char a[]){ + + int r=rpm; + + if(r==0)a[0]=a[1]=0xff; + else if(r<3000){a[0]=0xfb;a[1]=0xef;} + else if(r<(3000+1333)){a[0]=0xf3;a[1]=0b11001111;} + else if(r<(3000+1333*2)){a[0]=0xe3;a[1]=0b11000111;} + else if(r<(3000+1333*3)){a[0]=0xc3;a[1]=0b11000011;} + else if(r<(3000+1333*4)){a[0]=0x83;a[1]=0b11000001;} + else if(r<(3000+1333*5)){a[0]=0x03;a[1]=0b11000000;} + else if(r<(11000)){a[0]=0x01;a[1]=0b10000000;} + else {a[0]=0x00;a[1]=0x00;} +} + + +bool trasmetti::pc_trasmisione(int n,char* s) +{// da implementae con funzione che invia i dati su +char ss[30]; + +for(int i=0;i<20;i++)ss[5+i]='X'; +strcpy(ss,s); + + +/*da portare in setting e trovare un modo epr farli risultare qui*/ +const int addr = 0x70; +const int addr1 = 0x72; + + + +char nn[2]; + +char a[2]; + +nn[0]=22; +int rpm; + +//master->frequency(100000);//da portare in setting e non farlo sempre + +//aggiungere condizione di return false se il write fallisce ! + wait_us(10); + + master->write(addr_slave,ss,nn[0]); + wait_us(1); + master->stop(); + + + #define led_genny + + #ifdef led_genny + if(s[0]=='D'){ + rpm=(s[7]-'0')*10000+(s[8]-'0')*1000+(s[9]-'0')*100+(s[10]-'0')*10+s[11]-'0'; + led_rpm(rpm,a); + master->write(addr, a, 1); // Send command string + master->stop(); + master->write(addr1, &a[1], 1); // Send command string + master->stop(); + } + + + #endif + + + // device.printf("%s@",s); + // device2.printf("%s@",s); + + // pc.printf("%s\r\n",ss); + + +return true; +} +#endif +} +#endif \ No newline at end of file