12_TCPIP - there is a problem. i cant send data continuously.

Users » fyazgan » Code » 12_TCPIP

Fevzi YAZGAN / Mbed 2 deprecated 12_TCPIP

there is a problem. i cant send data continuously.

Dependencies: EthernetNetIf mbed HMC6352 ITG3200 ADXL345 IMUfilter

IMUTICKER/IMUTICKER.h@0:711905e937b9, 2011-07-24 (annotated)

Committer:: fyazgan
Date:: Sun Jul 24 19:49:51 2011 +0000
Revision:: 0:711905e937b9

Who changed what in which revision?

User	Revision	Line number	New contents of line
fyazgan	0:711905e937b9	1	/**
fyazgan	0:711905e937b9	2	* IMU filter example.
fyazgan	0:711905e937b9	3	*
fyazgan	0:711905e937b9	4	* Calculate the roll, pitch and yaw angles.
fyazgan	0:711905e937b9	5	*/
fyazgan	0:711905e937b9	6	#include "IMUfilter.h"
fyazgan	0:711905e937b9	7	#include "ADXL345.h"
fyazgan	0:711905e937b9	8	#include "ITG3200.h"
fyazgan	0:711905e937b9	9
fyazgan	0:711905e937b9	10	//Gravity at Earth's surface in m/s/s
fyazgan	0:711905e937b9	11	#define g0 9.812865328
fyazgan	0:711905e937b9	12	//Number of samples to average.
fyazgan	0:711905e937b9	13	#define SAMPLES 4
fyazgan	0:711905e937b9	14	//Number of samples to be averaged for a null bias calculation
fyazgan	0:711905e937b9	15	//during calibration.
fyazgan	0:711905e937b9	16	#define CALIBRATION_SAMPLES 128
fyazgan	0:711905e937b9	17	//Convert from radians to degrees.
fyazgan	0:711905e937b9	18	#define toDegrees(x) (x * 57.2957795)
fyazgan	0:711905e937b9	19	//Convert from degrees to radians.
fyazgan	0:711905e937b9	20	#define toRadians(x) (x * 0.01745329252)
fyazgan	0:711905e937b9	21	//ITG-3200 sensitivity is 14.375 LSB/(degrees/sec).
fyazgan	0:711905e937b9	22	#define GYROSCOPE_GAIN (1 / 14.375)
fyazgan	0:711905e937b9	23	//Full scale resolution on the ADXL345 is 4mg/LSB.
fyazgan	0:711905e937b9	24	#define ACCELEROMETER_GAIN (0.004 * g0)
fyazgan	0:711905e937b9	25	//Sampling gyroscope at 200Hz.
fyazgan	0:711905e937b9	26	#define GYRO_RATE 0.005
fyazgan	0:711905e937b9	27	//Sampling accelerometer at 200Hz.
fyazgan	0:711905e937b9	28	#define ACC_RATE 0.005
fyazgan	0:711905e937b9	29	//Updating filter at 40Hz.
fyazgan	0:711905e937b9	30	#define FILTER_RATE 0.1
fyazgan	0:711905e937b9	31
fyazgan	0:711905e937b9	32	Serial pc(USBTX, USBRX);
fyazgan	0:711905e937b9	33	//At rest the gyroscope is centred around 0 and goes between about
fyazgan	0:711905e937b9	34	//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
fyazgan	0:711905e937b9	35	//5/15 = 0.3 degrees/sec.
fyazgan	0:711905e937b9	36	IMUfilter imuFilter(FILTER_RATE, 0.3);
fyazgan	0:711905e937b9	37	ADXL345 accelerometer(p5, p6, p7, p8);
fyazgan	0:711905e937b9	38	ITG3200 gyroscope(p9, p10);
fyazgan	0:711905e937b9	39	Ticker accelerometerTicker;
fyazgan	0:711905e937b9	40	Ticker gyroscopeTicker;
fyazgan	0:711905e937b9	41	Ticker filterTicker;
fyazgan	0:711905e937b9	42
fyazgan	0:711905e937b9	43	//Offsets for the gyroscope.
fyazgan	0:711905e937b9	44	//The readings we take when the gyroscope is stationary won't be 0, so we'll
fyazgan	0:711905e937b9	45	//average a set of readings we do get when the gyroscope is stationary and
fyazgan	0:711905e937b9	46	//take those away from subsequent readings to ensure the gyroscope is offset
fyazgan	0:711905e937b9	47	//or "biased" to 0.
fyazgan	0:711905e937b9	48	double w_xBias;
fyazgan	0:711905e937b9	49	double w_yBias;
fyazgan	0:711905e937b9	50	double w_zBias;
fyazgan	0:711905e937b9	51
fyazgan	0:711905e937b9	52	//Offsets for the accelerometer.
fyazgan	0:711905e937b9	53	//Same as with the gyroscope.
fyazgan	0:711905e937b9	54	double a_xBias;
fyazgan	0:711905e937b9	55	double a_yBias;
fyazgan	0:711905e937b9	56	double a_zBias;
fyazgan	0:711905e937b9	57
fyazgan	0:711905e937b9	58	//Accumulators used for oversampling and then averaging.
fyazgan	0:711905e937b9	59	volatile double a_xAccumulator = 0;
fyazgan	0:711905e937b9	60	volatile double a_yAccumulator = 0;
fyazgan	0:711905e937b9	61	volatile double a_zAccumulator = 0;
fyazgan	0:711905e937b9	62	volatile double w_xAccumulator = 0;
fyazgan	0:711905e937b9	63	volatile double w_yAccumulator = 0;
fyazgan	0:711905e937b9	64	volatile double w_zAccumulator = 0;
fyazgan	0:711905e937b9	65
fyazgan	0:711905e937b9	66	//Accelerometer and gyroscope readings for x, y, z axes.
fyazgan	0:711905e937b9	67	volatile double a_x;
fyazgan	0:711905e937b9	68	volatile double a_y;
fyazgan	0:711905e937b9	69	volatile double a_z;
fyazgan	0:711905e937b9	70	volatile double w_x;
fyazgan	0:711905e937b9	71	volatile double w_y;
fyazgan	0:711905e937b9	72	volatile double w_z;
fyazgan	0:711905e937b9	73
fyazgan	0:711905e937b9	74	//Buffer for accelerometer readings.
fyazgan	0:711905e937b9	75	int readings[3];
fyazgan	0:711905e937b9	76	//Number of accelerometer samples we're on.
fyazgan	0:711905e937b9	77	int accelerometerSamples = 0;
fyazgan	0:711905e937b9	78	//Number of gyroscope samples we're on.
fyazgan	0:711905e937b9	79	int gyroscopeSamples = 0;
fyazgan	0:711905e937b9	80
fyazgan	0:711905e937b9	81	/**
fyazgan	0:711905e937b9	82	* Prototypes
fyazgan	0:711905e937b9	83	*/
fyazgan	0:711905e937b9	84	//Set up the ADXL345 appropriately.
fyazgan	0:711905e937b9	85	void initializeAcceleromter(void);
fyazgan	0:711905e937b9	86	//Calculate the null bias.
fyazgan	0:711905e937b9	87	void calibrateAccelerometer(void);
fyazgan	0:711905e937b9	88	//Take a set of samples and average them.
fyazgan	0:711905e937b9	89	void sampleAccelerometer(void);
fyazgan	0:711905e937b9	90	//Set up the ITG3200 appropriately.
fyazgan	0:711905e937b9	91	void initializeGyroscope(void);
fyazgan	0:711905e937b9	92	//Calculate the null bias.
fyazgan	0:711905e937b9	93	void calibrateGyroscope(void);
fyazgan	0:711905e937b9	94	//Take a set of samples and average them.
fyazgan	0:711905e937b9	95	void sampleGyroscope(void);
fyazgan	0:711905e937b9	96	//Update the filter and calculate the Euler angles.
fyazgan	0:711905e937b9	97	void filter(void);
fyazgan	0:711905e937b9	98
fyazgan	0:711905e937b9	99	void initializeAccelerometer(void) {
fyazgan	0:711905e937b9	100
fyazgan	0:711905e937b9	101	//Go into standby mode to configure the device.
fyazgan	0:711905e937b9	102	accelerometer.setPowerControl(0x00);
fyazgan	0:711905e937b9	103	//Full resolution, +/-16g, 4mg/LSB.
fyazgan	0:711905e937b9	104	accelerometer.setDataFormatControl(0x0B);
fyazgan	0:711905e937b9	105	//200Hz data rate.
fyazgan	0:711905e937b9	106	accelerometer.setDataRate(ADXL345_200HZ);
fyazgan	0:711905e937b9	107	//Measurement mode.
fyazgan	0:711905e937b9	108	accelerometer.setPowerControl(0x08);
fyazgan	0:711905e937b9	109	//See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
fyazgan	0:711905e937b9	110	wait_ms(22);
fyazgan	0:711905e937b9	111
fyazgan	0:711905e937b9	112	}
fyazgan	0:711905e937b9	113
fyazgan	0:711905e937b9	114	void sampleAccelerometer(void) {
fyazgan	0:711905e937b9	115
fyazgan	0:711905e937b9	116	//Have we taken enough samples?
fyazgan	0:711905e937b9	117	if (accelerometerSamples == SAMPLES) {
fyazgan	0:711905e937b9	118
fyazgan	0:711905e937b9	119	//Average the samples, remove the bias, and calculate the acceleration
fyazgan	0:711905e937b9	120	//in m/s/s.
fyazgan	0:711905e937b9	121	a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
fyazgan	0:711905e937b9	122	a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
fyazgan	0:711905e937b9	123	a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
fyazgan	0:711905e937b9	124
fyazgan	0:711905e937b9	125	a_xAccumulator = 0;
fyazgan	0:711905e937b9	126	a_yAccumulator = 0;
fyazgan	0:711905e937b9	127	a_zAccumulator = 0;
fyazgan	0:711905e937b9	128	accelerometerSamples = 0;
fyazgan	0:711905e937b9	129
fyazgan	0:711905e937b9	130	} else {
fyazgan	0:711905e937b9	131	//Take another sample.
fyazgan	0:711905e937b9	132	accelerometer.getOutput(readings);
fyazgan	0:711905e937b9	133
fyazgan	0:711905e937b9	134	a_xAccumulator += (int16_t) readings[0];
fyazgan	0:711905e937b9	135	a_yAccumulator += (int16_t) readings[1];
fyazgan	0:711905e937b9	136	a_zAccumulator += (int16_t) readings[2];
fyazgan	0:711905e937b9	137
fyazgan	0:711905e937b9	138	accelerometerSamples++;
fyazgan	0:711905e937b9	139
fyazgan	0:711905e937b9	140	}
fyazgan	0:711905e937b9	141
fyazgan	0:711905e937b9	142	}
fyazgan	0:711905e937b9	143
fyazgan	0:711905e937b9	144	void calibrateAccelerometer(void) {
fyazgan	0:711905e937b9	145
fyazgan	0:711905e937b9	146	a_xAccumulator = 0;
fyazgan	0:711905e937b9	147	a_yAccumulator = 0;
fyazgan	0:711905e937b9	148	a_zAccumulator = 0;
fyazgan	0:711905e937b9	149
fyazgan	0:711905e937b9	150	//Take a number of readings and average them
fyazgan	0:711905e937b9	151	//to calculate the zero g offset.
fyazgan	0:711905e937b9	152	for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
fyazgan	0:711905e937b9	153
fyazgan	0:711905e937b9	154	accelerometer.getOutput(readings);
fyazgan	0:711905e937b9	155
fyazgan	0:711905e937b9	156	a_xAccumulator += (int16_t) readings[0];
fyazgan	0:711905e937b9	157	a_yAccumulator += (int16_t) readings[1];
fyazgan	0:711905e937b9	158	a_zAccumulator += (int16_t) readings[2];
fyazgan	0:711905e937b9	159
fyazgan	0:711905e937b9	160	wait(ACC_RATE);
fyazgan	0:711905e937b9	161
fyazgan	0:711905e937b9	162	}
fyazgan	0:711905e937b9	163
fyazgan	0:711905e937b9	164	a_xAccumulator /= CALIBRATION_SAMPLES;
fyazgan	0:711905e937b9	165	a_yAccumulator /= CALIBRATION_SAMPLES;
fyazgan	0:711905e937b9	166	a_zAccumulator /= CALIBRATION_SAMPLES;
fyazgan	0:711905e937b9	167
fyazgan	0:711905e937b9	168	//At 4mg/LSB, 250 LSBs is 1g.
fyazgan	0:711905e937b9	169	a_xBias = a_xAccumulator;
fyazgan	0:711905e937b9	170	a_yBias = a_yAccumulator;
fyazgan	0:711905e937b9	171	a_zBias = (a_zAccumulator - 250);
fyazgan	0:711905e937b9	172
fyazgan	0:711905e937b9	173	a_xAccumulator = 0;
fyazgan	0:711905e937b9	174	a_yAccumulator = 0;
fyazgan	0:711905e937b9	175	a_zAccumulator = 0;
fyazgan	0:711905e937b9	176
fyazgan	0:711905e937b9	177	}
fyazgan	0:711905e937b9	178
fyazgan	0:711905e937b9	179	void initializeGyroscope(void) {
fyazgan	0:711905e937b9	180
fyazgan	0:711905e937b9	181	//Low pass filter bandwidth of 42Hz.
fyazgan	0:711905e937b9	182	gyroscope.setLpBandwidth(LPFBW_42HZ);
fyazgan	0:711905e937b9	183	//Internal sample rate of 200Hz. (1kHz / 5).
fyazgan	0:711905e937b9	184	gyroscope.setSampleRateDivider(4);
fyazgan	0:711905e937b9	185
fyazgan	0:711905e937b9	186	}
fyazgan	0:711905e937b9	187
fyazgan	0:711905e937b9	188	void calibrateGyroscope(void) {
fyazgan	0:711905e937b9	189
fyazgan	0:711905e937b9	190	w_xAccumulator = 0;
fyazgan	0:711905e937b9	191	w_yAccumulator = 0;
fyazgan	0:711905e937b9	192	w_zAccumulator = 0;
fyazgan	0:711905e937b9	193
fyazgan	0:711905e937b9	194	//Take a number of readings and average them
fyazgan	0:711905e937b9	195	//to calculate the gyroscope bias offset.
fyazgan	0:711905e937b9	196	for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
fyazgan	0:711905e937b9	197
fyazgan	0:711905e937b9	198	w_xAccumulator += gyroscope.getGyroX();
fyazgan	0:711905e937b9	199	w_yAccumulator += gyroscope.getGyroY();
fyazgan	0:711905e937b9	200	w_zAccumulator += gyroscope.getGyroZ();
fyazgan	0:711905e937b9	201	wait(GYRO_RATE);
fyazgan	0:711905e937b9	202
fyazgan	0:711905e937b9	203	}
fyazgan	0:711905e937b9	204
fyazgan	0:711905e937b9	205	//Average the samples.
fyazgan	0:711905e937b9	206	w_xAccumulator /= CALIBRATION_SAMPLES;
fyazgan	0:711905e937b9	207	w_yAccumulator /= CALIBRATION_SAMPLES;
fyazgan	0:711905e937b9	208	w_zAccumulator /= CALIBRATION_SAMPLES;
fyazgan	0:711905e937b9	209
fyazgan	0:711905e937b9	210	w_xBias = w_xAccumulator;
fyazgan	0:711905e937b9	211	w_yBias = w_yAccumulator;
fyazgan	0:711905e937b9	212	w_zBias = w_zAccumulator;
fyazgan	0:711905e937b9	213
fyazgan	0:711905e937b9	214	w_xAccumulator = 0;
fyazgan	0:711905e937b9	215	w_yAccumulator = 0;
fyazgan	0:711905e937b9	216	w_zAccumulator = 0;
fyazgan	0:711905e937b9	217
fyazgan	0:711905e937b9	218	}
fyazgan	0:711905e937b9	219
fyazgan	0:711905e937b9	220	void sampleGyroscope(void) {
fyazgan	0:711905e937b9	221
fyazgan	0:711905e937b9	222	//Have we taken enough samples?
fyazgan	0:711905e937b9	223	if (gyroscopeSamples == SAMPLES) {
fyazgan	0:711905e937b9	224
fyazgan	0:711905e937b9	225	//Average the samples, remove the bias, and calculate the angular
fyazgan	0:711905e937b9	226	//velocity in rad/s.
fyazgan	0:711905e937b9	227	w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
fyazgan	0:711905e937b9	228	w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
fyazgan	0:711905e937b9	229	w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
fyazgan	0:711905e937b9	230
fyazgan	0:711905e937b9	231	w_xAccumulator = 0;
fyazgan	0:711905e937b9	232	w_yAccumulator = 0;
fyazgan	0:711905e937b9	233	w_zAccumulator = 0;
fyazgan	0:711905e937b9	234	gyroscopeSamples = 0;
fyazgan	0:711905e937b9	235
fyazgan	0:711905e937b9	236	} else {
fyazgan	0:711905e937b9	237	//Take another sample.
fyazgan	0:711905e937b9	238	w_xAccumulator += gyroscope.getGyroX();
fyazgan	0:711905e937b9	239	w_yAccumulator += gyroscope.getGyroY();
fyazgan	0:711905e937b9	240	w_zAccumulator += gyroscope.getGyroZ();
fyazgan	0:711905e937b9	241
fyazgan	0:711905e937b9	242	gyroscopeSamples++;
fyazgan	0:711905e937b9	243
fyazgan	0:711905e937b9	244	}
fyazgan	0:711905e937b9	245
fyazgan	0:711905e937b9	246	}
fyazgan	0:711905e937b9	247
fyazgan	0:711905e937b9	248	void filter(void) {
fyazgan	0:711905e937b9	249
fyazgan	0:711905e937b9	250	//Update the filter variables.
fyazgan	0:711905e937b9	251	imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
fyazgan	0:711905e937b9	252	//Calculate the new Euler angles.
fyazgan	0:711905e937b9	253	imuFilter.computeEuler();
fyazgan	0:711905e937b9	254
fyazgan	0:711905e937b9	255	}
fyazgan	0:711905e937b9	256
fyazgan	0:711905e937b9	257	void start() {
fyazgan	0:711905e937b9	258
fyazgan	0:711905e937b9	259	pc.printf("Starting IMU filter test...\n");
fyazgan	0:711905e937b9	260
fyazgan	0:711905e937b9	261	//Initialize inertial sensors.
fyazgan	0:711905e937b9	262	initializeAccelerometer();
fyazgan	0:711905e937b9	263	calibrateAccelerometer();
fyazgan	0:711905e937b9	264	initializeGyroscope();
fyazgan	0:711905e937b9	265	calibrateGyroscope();
fyazgan	0:711905e937b9	266
fyazgan	0:711905e937b9	267
fyazgan	0:711905e937b9	268	//Set up timers.
fyazgan	0:711905e937b9	269	//Accelerometer data rate is 200Hz, so we'll sample at this speed.
fyazgan	0:711905e937b9	270	accelerometerTicker.attach(&sampleAccelerometer, 0.005);
fyazgan	0:711905e937b9	271	//Gyroscope data rate is 200Hz, so we'll sample at this speed.
fyazgan	0:711905e937b9	272	gyroscopeTicker.attach(&sampleGyroscope, 0.005);
fyazgan	0:711905e937b9	273	//Update the filter variables at the correct rate.
fyazgan	0:711905e937b9	274	filterTicker.attach(&filter, FILTER_RATE);
fyazgan	0:711905e937b9	275
fyazgan	0:711905e937b9	276
fyazgan	0:711905e937b9	277	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	24 Jul 2011
Imports:	3
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	6
Followers:	1

IMUTICKER/IMUTICKER.h@0:711905e937b9, 2011-07-24 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning