Mini-X - | Mbed

Users » agiembed » Code » Mini-X

2gcs.h@0:d463d5c04541, 2011-08-16 (annotated)

Committer:: agiembed
Date:: Tue Aug 16 05:32:33 2011 +0000
Revision:: 0:d463d5c04541

Who changed what in which revision?

User	Revision	Line number	New contents of line
agiembed	0:d463d5c04541	1	#define FCC_DATA_SIZE 52 //39 Direct from IMU
agiembed	0:d463d5c04541	2	#define GPS_DATA_SIZE 20
agiembed	0:d463d5c04541	3
agiembed	0:d463d5c04541	4	char lati[6], altig[6];
agiembed	0:d463d5c04541	5
agiembed	0:d463d5c04541	6	float llt = 127.1234; //POSRoll;//dlat;// gps.lat; gps.lat =
agiembed	0:d463d5c04541	7	float lng = 37.4321; //POSPitch; //dlon;// gps.lon; gps.lon =
agiembed	0:d463d5c04541	8
agiembed	0:d463d5c04541	9	void IMUtoGCS(){
agiembed	0:d463d5c04541	10
agiembed	0:d463d5c04541	11	char FtoG = (char )&fcc, i; //&IMU
agiembed	0:d463d5c04541	12
agiembed	0:d463d5c04541	13	radio.putc(0xFF);
agiembed	0:d463d5c04541	14	radio.putc(0xFF);
agiembed	0:d463d5c04541	15
agiembed	0:d463d5c04541	16	fcc.acc_y = roll_con; // llt;
agiembed	0:d463d5c04541	17	fcc.acc_z = pitch_con;// lng;
agiembed	0:d463d5c04541	18
agiembed	0:d463d5c04541	19	if(gps.lat>0) fcc.acc_x = 1; else fcc.acc_x = 0;
agiembed	0:d463d5c04541	20
agiembed	0:d463d5c04541	21	// fcc.IR2 = (unsigned short)F_range;
agiembed	0:d463d5c04541	22	//fcc.IR1 = F_error;
agiembed	0:d463d5c04541	23
agiembed	0:d463d5c04541	24	llt += 0.0001;
agiembed	0:d463d5c04541	25	if(llt > 128.00) llt = 127.1234;
agiembed	0:d463d5c04541	26	lng += 0.0001;
agiembed	0:d463d5c04541	27	if(lng > 38.00) lng = 37.4321;
agiembed	0:d463d5c04541	28
agiembed	0:d463d5c04541	29
agiembed	0:d463d5c04541	30
agiembed	0:d463d5c04541	31	for (i=0;i<FCC_DATA_SIZE;i++) radio.putc(*FtoG++);
agiembed	0:d463d5c04541	32	radio.putc(0x0D);
agiembed	0:d463d5c04541	33	return;
agiembed	0:d463d5c04541	34	}
agiembed	0:d463d5c04541	35
agiembed	0:d463d5c04541	36	void GPStoGCS(){
agiembed	0:d463d5c04541	37
agiembed	0:d463d5c04541	38	char FtoG = (char )&gps, i; //&IMU
agiembed	0:d463d5c04541	39
agiembed	0:d463d5c04541	40	radio.putc(0xFE);
agiembed	0:d463d5c04541	41	radio.putc(0xFE);
agiembed	0:d463d5c04541	42
agiembed	0:d463d5c04541	43	for (i=0;i<GPS_DATA_SIZE;i++) radio.putc(*FtoG++);
agiembed	0:d463d5c04541	44	radio.putc(0x0D);
agiembed	0:d463d5c04541	45	return;
agiembed	0:d463d5c04541	46	}
agiembed	0:d463d5c04541	47
agiembed	0:d463d5c04541	48	void GPSASCII(){
agiembed	0:d463d5c04541	49
agiembed	0:d463d5c04541	50	radio.printf("%f ", dlat);
agiembed	0:d463d5c04541	51	radio.printf("%f ", dlon);
agiembed	0:d463d5c04541	52	//radio.printf("%d ", FMD.lock);
agiembed	0:d463d5c04541	53	radio.printf("%f ", gps.lat);
agiembed	0:d463d5c04541	54	// radio.printf("%d\n", FMD.mode);
agiembed	0:d463d5c04541	55	radio.printf("%f ", gps.lon);
agiembed	0:d463d5c04541	56	/// radio.printf("%d\n", FMD.mission);
agiembed	0:d463d5c04541	57	radio.printf("%f\n ", gps.alti);
agiembed	0:d463d5c04541	58
agiembed	0:d463d5c04541	59
agiembed	0:d463d5c04541	60
agiembed	0:d463d5c04541	61	//radio.printf("%f ", IMU.roll);
agiembed	0:d463d5c04541	62	//radio.printf("%f ", IMU.pitch);
agiembed	0:d463d5c04541	63	//radio.printf("%f\n", IMU.yaw);
agiembed	0:d463d5c04541	64
agiembed	0:d463d5c04541	65	// radio.printf("%f", gps.dist);
agiembed	0:d463d5c04541	66	//radio.printf(" ");
agiembed	0:d463d5c04541	67	//radio.printf("%f", gps.bear);
agiembed	0:d463d5c04541	68	//radio.printf("\n");
agiembed	0:d463d5c04541	69	}
agiembed	0:d463d5c04541	70
agiembed	0:d463d5c04541	71	void toBHG(){
agiembed	0:d463d5c04541	72
agiembed	0:d463d5c04541	73	char FtoG = (char )&fcc, i; //&IMU
agiembed	0:d463d5c04541	74
agiembed	0:d463d5c04541	75	pc.putc(0xFF);
agiembed	0:d463d5c04541	76	pc.putc(0xFF);
agiembed	0:d463d5c04541	77	for (i=0;i<FCC_DATA_SIZE;i++) pc.putc(*FtoG++);
agiembed	0:d463d5c04541	78	pc.putc(0x0D);
agiembed	0:d463d5c04541	79	}
agiembed	0:d463d5c04541	80
agiembed	0:d463d5c04541	81	void rcprint(){
agiembed	0:d463d5c04541	82
agiembed	0:d463d5c04541	83	printf("%d\t", RC.rolls);
agiembed	0:d463d5c04541	84	printf("%d\t", RC.throttles);
agiembed	0:d463d5c04541	85	printf("%d\t", RC.pitchs);
agiembed	0:d463d5c04541	86	printf("%d\t", RC.yaws);
agiembed	0:d463d5c04541	87	printf("%d\n", RC.sws);
agiembed	0:d463d5c04541	88	return;
agiembed	0:d463d5c04541	89	}
agiembed	0:d463d5c04541	90
agiembed	0:d463d5c04541	91	void imuprint(){
agiembed	0:d463d5c04541	92	printf("%d\t", (int)fcc.roll);
agiembed	0:d463d5c04541	93	printf("%d\t", (int)fcc.pitch);
agiembed	0:d463d5c04541	94	printf("%d\n", (int)fcc.yaw);
agiembed	0:d463d5c04541	95	return;
agiembed	0:d463d5c04541	96	}
agiembed	0:d463d5c04541	97
agiembed	0:d463d5c04541	98	/*void motprint(){
agiembed	0:d463d5c04541	99	printf("%d\t", bl_F[1]);
agiembed	0:d463d5c04541	100	printf("%d\t", bl_B[1]);
agiembed	0:d463d5c04541	101	printf("%d\t", bl_R[1]);
agiembed	0:d463d5c04541	102	printf("%d\n", bl_L[1]);
agiembed	0:d463d5c04541	103	return;
agiembed	0:d463d5c04541	104	}
agiembed	0:d463d5c04541	105
agiembed	0:d463d5c04541	106	void xmotprint(){
agiembed	0:d463d5c04541	107	printf("%d\t", front_bl);
agiembed	0:d463d5c04541	108	printf("%d\t", back_bl);
agiembed	0:d463d5c04541	109	printf("%d\t", right_bl);
agiembed	0:d463d5c04541	110	printf("%d\n", left_bl);
agiembed	0:d463d5c04541	111	return;
agiembed	0:d463d5c04541	112	}*/
agiembed	0:d463d5c04541	113
agiembed	0:d463d5c04541	114	void pidprint(){
agiembed	0:d463d5c04541	115	printf("%d\t", alti_con);
agiembed	0:d463d5c04541	116	printf("%d\t", roll_con);
agiembed	0:d463d5c04541	117	printf("%d\t", pitch_con);
agiembed	0:d463d5c04541	118	printf("%d\n", yaw_con);
agiembed	0:d463d5c04541	119	return;
agiembed	0:d463d5c04541	120	}
agiembed	0:d463d5c04541	121
agiembed	0:d463d5c04541	122	void gainprint(){
agiembed	0:d463d5c04541	123	printf("%d\t", (int)gainRoll.p);
agiembed	0:d463d5c04541	124	printf("%d\t", (int)gainRoll.i);
agiembed	0:d463d5c04541	125	printf("%d\t", (int)gainRoll.d);
agiembed	0:d463d5c04541	126	printf("%d\n", (int)gainRoll.r);
agiembed	0:d463d5c04541	127	return;
agiembed	0:d463d5c04541	128	}
agiembed	0:d463d5c04541	129
agiembed	0:d463d5c04541	130
agiembed	0:d463d5c04541	131	void yawprint(){
agiembed	0:d463d5c04541	132
agiembed	0:d463d5c04541	133	printf("%d\t", RC.yaws);
agiembed	0:d463d5c04541	134	printf("%d\t", (short)fcc.yaw);
agiembed	0:d463d5c04541	135	printf("%d\n", (short)Yaw_ref);
agiembed	0:d463d5c04541	136	return;
agiembed	0:d463d5c04541	137	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	16 Aug 2011
Imports:	0
Forks:	0
Commits:	1
Dependents:	0
Dependencies:	0
Followers:	1

2gcs.h@0:d463d5c04541, 2011-08-16 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning