main.cpp

00001 #include "mbed.h"
00002 #include "mavlink/mavlink.h"
00003 
00004 //DigitalOut myled(LED_GREEN);
00005 Serial vr(p28, p27);
00006 
00007 const int bufSize = 31;
00008 
00009 struct StepvrMessage {
00010     float x;
00011     float y;
00012     float z;
00013     float q1;
00014     float q2;
00015     float q3;
00016     float q4;
00017 };
00018 
00019 
00020 
00021 void syncFrame(void) {
00022     unsigned char a = 0;
00023     while (a != 0x5a) {
00024         if(vr.readable())
00025             a=vr.getc();
00026     }
00027 
00028     int readCount = 1;
00029     while (readCount < bufSize) {
00030         if(vr.readable()){
00031             a=vr.getc();
00032             readCount++;
00033         }
00034     }
00035 }
00036 
00037 
00038 
00039 
00040 #define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
00041 
00042 uint64_t microsSinceEpoch(){
00043     return 0;
00044 }
00045 
00046 
00047 
00048 int main()
00049 {
00050     
00051     uint8_t mavBuf[BUFFER_LENGTH];
00052 
00053     mavlink_message_t mavMsg;
00054     uint16_t len;
00055 
00056 
00057 
00058     vr.baud(921600);
00059     
00060     
00061     
00062 //    while(1){
00063 //      if(vr.readable()){
00064 //            unsigned char w = vr.getc();
00065 //            vr.putc(w);
00066 //        }
00067 //    }
00068     
00069     
00070     syncFrame();
00071     vr.putc('!');
00072     
00073     unsigned char vrBuf[bufSize];
00074     
00075     int readCount = 0;
00076     while(1) {
00077         if(vr.readable()){
00078             vrBuf[readCount]=vr.getc();
00079             readCount++;
00080         }
00081         //todo:check sum?
00082         if (readCount == bufSize){
00083             if ( (*(unsigned char *)vrBuf) == 0x5a ) {
00084                 StepvrMessage *vrMsg = (StepvrMessage *) (vrBuf + 1);
00085                 
00086 //              /*Send Heartbeat */
00087 //              mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_AUTOPILOT_GENERIC, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
00088 //              len = mavlink_msg_to_send_buffer(buf, &msg);
00089 //              pr(buf,len);
00090 
00091                 float q[4]={vrMsg->q1,vrMsg->q4,-vrMsg->q2,-vrMsg->q3};
00092                 mavlink_msg_att_pos_mocap_pack(1,200,&mavMsg,microsSinceEpoch(),q,vrMsg->z,-vrMsg->x,-vrMsg->y);
00093                 len = mavlink_msg_to_send_buffer(mavBuf, &mavMsg);
00094                 for(int i=0;i<len;){
00095                     if(vr.writeable())
00096                     vr.putc(mavBuf[i++]);  
00097                 }
00098             }
00099             else{
00100                 syncFrame();
00101             }
00102             readCount = 0;
00103         }
00104     }
00105 }