MAVLink_API.cpp

00001 #include "MAVLink_API.h"
00002 
00003 #if DEBUG
00004 Serial debug(USBTX, USBRX);
00005 #endif
00006 
00007 mavLink_API::mavLink_API(float heartbeat_freq){
00008     mavlink_system.sysid = 20;                   ///< ID 20 for this airplane
00009 //mavlink_system.compid = MAV_COMP_ID_IMU;     ///< The component sending the message is the IMU, it could be also a Linux process
00010     mavlink_system.compid = MAV_COMP_ID_IMU;
00011     mavlink_system.type = MAV_QUADROTOR;
00012     
00013     buf = new uint8_t[MAVLINK_MAX_PACKET_LEN];
00014     messageReadyToBeSent = false;
00015     heartBeatTicker.attach(this, &mavLink_API::sendHeartBeat, heartbeat_freq);
00016 #if DEBUG
00017     debug.printf("Object MAVLinK_API instanciated\n");
00018 #endif
00019 }
00020 
00021 mavLink_API::~mavLink_API(){
00022     
00023 }
00024 
00025 void mavLink_API::sendHeartBeat()
00026 {
00027     
00028 #if DEBUG
00029     debug.printf("Send heartBeat\n");
00030 #endif
00031     uint8_t system_type = MAV_QUADROTOR;
00032     uint8_t autopilot_type = MAV_AUTOPILOT_GENERIC;
00033  
00034     uint8_t system_mode = MAV_MODE_READY; ///< Booting up
00035     uint32_t custom_mode = 0;                 ///< Custom mode, can be defined by user/adopter
00036     uint8_t system_state = MAV_STATE_STANDBY; ///< System ready for flight
00037  
00038 // Initialize the required buffers
00039     mavlink_message_t msg;
00040  
00041 // Pack the message
00042     mavlink_msg_heartbeat_pack(mavlink_system.sysid, mavlink_system.compid, &msg, system_type, autopilot_type, system_mode, custom_mode, system_state);
00043  
00044 // Copy the message to the send buffer
00045     len = mavlink_msg_to_send_buffer(buf, &msg);
00046     messageReadyToBeSent = true;
00047 }
00048 
00049 
00050 int mavLink_API::getMessage(char received_char){
00051 #if DEBUG
00052     //debug.printf("%X ", received_char);
00053 #endif
00054     
00055     if(mavlink_parse_char(MAVLINK_COMM_0, received_char, &received_msg, &mavlink_status)){
00056         //now handle the message received
00057         
00058 #if DEBUG
00059         //debug.printf("\n");
00060         debug.printf("Received MavLink message :\n");
00061         printMavLinkMessage(received_msg);
00062 #endif
00063         switch(received_msg.msgid)
00064         {
00065             case MAVLINK_MSG_ID_HEARTBEAT:
00066 #if DEBUG
00067             debug.printf("received heartbeat\n");
00068 #endif
00069             break;
00070             case MAVLINK_MSG_ID_COMMAND:
00071 #if DEBUG
00072             debug.printf("received command\n");
00073 #endif
00074             break;
00075             case MAVLINK_MSG_ID_SET_MODE:
00076 #if DEBUG
00077             debug.printf("received set mode\n");
00078             //target_system   uint8_t The system setting the mode
00079             //base_mode   uint8_t The new base mode
00080             //custom_mode
00081 #endif
00082             break;
00083             case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
00084 #if DEBUG
00085             debug.printf("received param request list\n");
00086 #endif
00087             break;
00088             default:
00089 #if DEBUG
00090 
00091 #endif
00092             break;
00093          }
00094         
00095     }
00096     else return 0;
00097     return 1;
00098 }
00099 int mavLink_API::handleMessage()
00100 {
00101     return 1;
00102 }
00103 
00104 #if DEBUG
00105 void mavLink_API::printMavLinkMessage(mavlink_message_t message)
00106 {
00107     debug.printf("Len :%d\n", message.len);     ///< Length of payload
00108     debug.printf("Seq :%d\n", message.seq);     ///< Sequence of packet
00109     debug.printf("Sysid :%d\n", message.sysid);   ///< ID of message sender system/aircraft
00110     debug.printf("Compid :%d\n", message.compid);  ///< ID of the message sender component
00111     debug.printf("MsgId :%d\n", message.msgid);   ///< ID of message in payload
00112     for(int i = 0 ; i < message.len ; i++){
00113         debug.printf("%X ", message.payload[i]);
00114     }
00115     debug.printf("\n");
00116     debug.printf("CK_a :%d\n", message.ck_a);    ///< Checksum high byte
00117     debug.printf("CK_b :%d\n", message.ck_b);    ///< Checksum low byte
00118 }
00119 #endif
00120 
00121 int createMessage(int messageID, void* data){
00122     return 1;
00123 }