Raharja Agie
Read IMU data at Serial port (p28, p27), data rate <100Hz @115200bps Read 5 channels PPM signal at p11 using InterruptIn Send data via RF Modem at serial port (p13, p14) in 36Hz @115200 Drive i2C motor speed controller at I2C port (p9, p10) in every 10mS (control loop delay time)
Revision 0:bd282c11d296, committed 2010-08-12
- Comitter:
- agiembed
- Date:
- Thu Aug 12 01:08:40 2010 +0000
- Commit message:
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/2gcs.h Thu Aug 12 01:08:40 2010 +0000
@@ -0,0 +1,18 @@
+#define FCC_DATA_SIZE 52 //39 Direct from IMU
+
+void toGCS(){
+
+
+ fcc.US++;// = 11;
+ if(fcc.US>1000) fcc.US = 0;
+
+ char *FtoG = (char *)&fcc, i; //&IMU
+
+
+ radio.putc(0xFF);
+ radio.putc(0xFF);
+ for (i=0;i<FCC_DATA_SIZE;i++) radio.putc(*FtoG++);
+ radio.putc(0x0D);
+
+ myled = !myled;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ahrs.h Thu Aug 12 01:08:40 2010 +0000
@@ -0,0 +1,72 @@
+#define Ahrs_num 40
+#define start 0
+#define head1 0xFF
+#define head2 0xFF
+#define check1 1
+#define check2 2
+
+unsigned char buf[1000], head[2], q[41], ck[1], data_idx = 0;
+char AHRS[Ahrs_num], check_ahrs = start, checkdata = 0, checksum;
+
+
+void IMU_Update(){
+ fcc.roll = IMU.roll;
+ fcc.pitch = IMU.pitch;
+ fcc.yaw = IMU.yaw;
+ fcc.g_roll = IMU.g_roll;
+ fcc.g_pitch = IMU.g_pitch;
+ fcc.g_yaw = IMU.g_yaw;
+ fcc.acc_x = IMU.acc_x;
+ fcc.acc_y = IMU.acc_y;
+ fcc.acc_z = IMU.acc_z;
+}
+
+
+void Get_AHRS(char *data){
+ int i;
+ char *dTemp = (char *) &IMU;
+
+ for(i=0; i<Ahrs_num; i++) *(dTemp+i) = *(data+i);
+ IMU_Update();
+}
+
+void ahrs_data(unsigned char data){
+
+ if(check_ahrs == start){
+ if(data == head1) check_ahrs = check1;
+ else check_ahrs = start;
+ }
+
+ else if(check_ahrs == check1){
+ if(data == head2) check_ahrs = check2;
+ else check_ahrs = start;
+ }
+
+ else if(check_ahrs == check2){
+ AHRS[data_idx] = data;
+ if(data_idx<(Ahrs_num-1)){
+ //checkdata ^= AHRS[data_idx];
+ data_idx++;
+ if(data_idx == 3) data_idx = 4;
+ }
+ else{
+ checksum = data;
+ //if(checksum == checkdata)
+ Get_AHRS(AHRS);
+ data_idx = 0;
+ check_ahrs = start;
+ }
+ }
+
+ else{
+ data_idx = 0;
+ check_ahrs = start;
+ }
+}
+
+void ahrs_rec(){
+ unsigned char data;
+ data = ahrs.getc();
+ ahrs_data(data);
+ //pc.putc(data);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/i2c.h Thu Aug 12 01:08:40 2010 +0000
@@ -0,0 +1,16 @@
+I2C i2c(p9, p10); //sda, scl
+
+const int addr = 0x50; // define the I2C Address
+char bl_F[2], bl_B[2], bl_L[2], bl_R[2];
+
+void eyetosee(){
+
+ bl_F[1]= 0;
+ bl_B[1]= 0;
+
+ i2c.write(addr+2, bl_L, 2); // Send command string to Motor Left
+ i2c.write(addr+4, bl_B, 2); // Send command string to Motor Back
+ i2c.write(addr+6, bl_F, 2); // Send command string to Motor Front
+ i2c.write(addr+8, bl_R, 2); // Send command string to Motor Right
+
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Aug 12 01:08:40 2010 +0000
@@ -0,0 +1,34 @@
+#include "mbed.h"
+#include "struct.h"
+
+short test, test1;
+
+#include "i2c.h"
+#include "rc.h"
+#include "setup.h"
+#include "2gcs.h"
+#include "ahrs.h"
+
+
+int main() {
+ setup();
+ //SendData.attach(&toGCS, 0.02777);
+ //Proccess.attach(&eyetosee, 0.1);
+ ahrs.attach(&ahrs_rec);
+ rc.rise(&PPM_rise); // attach the address of the PPM_rise function to the rising edge
+
+ while(1) { // wait around, interrupts will interrupt this!
+ printf("%d\t", RC.roll);
+ printf("%d\t", RC.throttle);
+ printf("%d\t", RC.pitch);
+ printf("%d\t", RC.yaw);
+ printf("%d\n", RC.sw);
+ wait(0.02);
+ //for(char i=0; i<6; i++){
+ // printf("%d\t", buf_ppm[i]);
+ // bl_L[1]= abs(test - test1);
+ // if(buf_ppm[i] > 5000) printf("%d\t%d\t%d\n", test, test1, bl_L[1]); //("%d\t%d\n", buf_ppm[i], i);
+ // wait(0.02);
+ // }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Aug 12 01:08:40 2010 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rc.h Thu Aug 12 01:08:40 2010 +0000
@@ -0,0 +1,92 @@
+Timer tick;
+InterruptIn rc(p11);
+short buf_ppm[6];
+char i = 0, idx, cnt = 0;
+
+
+void get_PPM(){
+ switch (idx){
+ case 1:
+ RC.roll = buf_ppm[2];
+ RC.throttle = buf_ppm[3];
+ RC.pitch = buf_ppm[4];
+ RC.yaw = buf_ppm[5];
+ RC.sw = buf_ppm[0];
+ break;
+
+ case 2:
+ RC.roll = buf_ppm[3];
+ RC.throttle = buf_ppm[4];
+ RC.pitch = buf_ppm[5];
+ RC.yaw = buf_ppm[0];
+ RC.sw = buf_ppm[1];
+ break;
+
+ case 3:
+ RC.roll = buf_ppm[4];
+ RC.throttle = buf_ppm[5];
+ RC.pitch = buf_ppm[0];
+ RC.yaw = buf_ppm[1];
+ RC.sw = buf_ppm[2];
+ break;
+
+ case 4:
+ RC.roll = buf_ppm[5];
+ RC.throttle = buf_ppm[0];
+ RC.pitch = buf_ppm[1];
+ RC.yaw = buf_ppm[2];
+ RC.sw = buf_ppm[3];
+ break;
+
+ case 5:
+ RC.roll = buf_ppm[0];
+ RC.throttle = buf_ppm[1];
+ RC.pitch = buf_ppm[2];
+ RC.yaw = buf_ppm[3];
+ RC.sw = buf_ppm[4];
+ break;
+
+ case 0:
+ RC.roll = buf_ppm[1];
+ RC.throttle = buf_ppm[2];
+ RC.pitch = buf_ppm[3];
+ RC.yaw = buf_ppm[4];
+ RC.sw = buf_ppm[5];
+ break;
+
+ default: break;
+ }
+ test = buf_ppm[2];
+ if(cnt<100)test1 = test;
+ return;
+}
+
+
+
+void gets_PPM(){
+ test = buf_ppm[2];
+ if(cnt<100)test1 = test;
+ //eyetosee();
+}
+
+void PPM_rise() {
+ tick.stop(); // Stop timer
+ buf_ppm[i]=tick.read_us(); // Read timer to buffer[i] i=turns
+ tick.reset(); // Reset timer
+ i++; // increment i.
+ if(buf_ppm[i]>5000) idx = i;
+ if(i==6) i = 0;
+ get_PPM();
+ cnt++;
+ if(cnt>200) cnt = 200;
+ tick.start();
+}
+
+
+/*
+Channel 1 = Roll
+Channel 2 = Throttle
+Channel 3 = Pitch
+Channel 4 = Yaw
+Channel 5 = Switch
+*/
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/setup.h Thu Aug 12 01:08:40 2010 +0000
@@ -0,0 +1,13 @@
+Serial pc(USBTX, USBRX);
+Serial ahrs(p28, p27);
+Serial radio(p13, p14);
+Ticker SendData;
+Ticker Proccess;
+DigitalOut myled(LED1);
+
+void setup(){
+ pc.baud(115200);
+ radio.baud(115200);
+ ahrs.baud(115200);
+}
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/struct.h Thu Aug 12 01:08:40 2010 +0000
@@ -0,0 +1,32 @@
+
+typedef struct _fcc_data {
+
+ unsigned short US; //2
+ unsigned short IR1; //2
+ unsigned short IR2; //2
+ unsigned short IR3; //2
+ unsigned short IR4; //2
+ unsigned short FCC_voltage; //2
+ char state_flag; //1
+ float roll, pitch, yaw; // 4, 4, 4
+ float g_roll, g_pitch, g_yaw; //4, 4, 4
+ float acc_x, acc_y, acc_z; //4, 4 ,4
+
+}FCC_DATA;
+FCC_DATA fcc;
+
+typedef struct _ahrs {
+
+ short packet; //2
+ unsigned char data_info; //1
+ float g_roll, g_pitch, g_yaw; //4, 4, 4
+ float acc_x, acc_y, acc_z; //4, 4 ,4
+ float roll, pitch, yaw; // 4, 4, 4
+
+}AHRS_DATA;
+AHRS_DATA IMU;
+
+typedef struct _rc {
+ short roll, throttle, pitch, yaw, sw;
+} RC_DATA;
+RC_DATA RC;
\ No newline at end of file