source: flair-src/branches/mavlink/tools/Controller/Mavlink/src/Mavlink_test.cpp @ 46

// %flair:license{
// This file is part of the Flair framework distributed under the
// CECILL-C License, Version 1.0.
// %flair:license}
//  created:    2016/07/05
//  filename:   Mavlink_test.cpp
//
//  author:     Thomas Fuhrmann
//              Copyright Heudiasyc UMR UTC/CNRS 7253
//
//  version:    $Id: $
//
//  purpose:    Base class for host side remote controls that talks to target
//  side through ethernet connection
//
//
/*********************************************************************/
#include "Mavlink_test.h"

#include <Controller.h>
#include <cvmatrix.h>
#include <Tab.h>
#include <TabWidget.h>
#include <CheckBox.h>
#include <Label.h>
#include <DataPlot1D.h>
#include <SpinBox.h>
#include <FrameworkManager.h>
#include <Socket.h>
#include <sstream>

#include <DoubleSpinBox.h>
#include <PushButton.h>
#include <ComboBox.h>
#include <GroupBox.h>

#include "mavlink.h"

using namespace flair::core;
using namespace flair::gui;
using namespace std;

namespace flair {
namespace sensor {

Mavlink::Mavlink(const FrameworkManager *parent, string name,
                  string receiverAddress, int receiverPort)
                  : Thread(parent, name, 6),
                  tab(new Tab(parent->GetTabWidget(), name)) {

  tabWidget = new TabWidget(tab->NewRow(), name);

  // Main tab
  Tab* settingsTab = new Tab(tabWidget, "Settings");
  
  // Controls group
  controlsGroupBox = new GroupBox(settingsTab->NewRow(), "Controls");
  btn_initialize = new PushButton(controlsGroupBox->LastRowLastCol(), "Initialize");
  btn_start_mission = new PushButton(controlsGroupBox->LastRowLastCol(), "Start mission");
  btn_stop_mission = new PushButton(controlsGroupBox->LastRowLastCol(), "Stop mission");
  btn_kill = new PushButton(controlsGroupBox->LastRowLastCol(), "Kill");

  // Add wpt group
  add_wptGroupBox = new GroupBox(settingsTab->NewRow(), "Add waypoint");
  latField = new DoubleSpinBox(add_wptGroupBox->NewRow(), "Latitude",
                                -90, 90, 0.0001, 4, 3);
  lonField = new DoubleSpinBox(add_wptGroupBox->LastRowLastCol(), "Longitude",
                                -180, 180, 0.0001, 4, 3);
  btn_add_mission_wpt = new PushButton(add_wptGroupBox->NewRow(), "Add mission wpt");
  btn_add_entrance_wpt = new PushButton(add_wptGroupBox->LastRowLastCol(), "Add entrance wpt");
  btn_add_exit_wpt = new PushButton(add_wptGroupBox->LastRowLastCol(), "Add exit wpt");

  // Show wpt group
  show_wptGroupBox = new GroupBox(settingsTab->NewRow(), "Show waypoints");
  label_mission_wpt = new Label(show_wptGroupBox->NewRow(), "Label mission");
  label_mission_wpt->SetText("Mission wpt");
  label_entrance_wpt = new Label(show_wptGroupBox->LastRowLastCol(), "Label entrance");
  label_entrance_wpt->SetText("Entrance wpt");
  label_exit_wpt = new Label(show_wptGroupBox->LastRowLastCol(), "Label exit");
  label_exit_wpt->SetText("Exit wpt");
  label_mission_wpt_list = new Label(show_wptGroupBox->NewRow(), "Label mission list");
  label_entrance_wpt_list = new Label(show_wptGroupBox->LastRowLastCol(), "Label entrance list");
  label_exit_wpt_list = new Label(show_wptGroupBox->LastRowLastCol(), "Label exit list");

  // Action wpt group
  action_wptGroupBox = new GroupBox(settingsTab->NewRow(), "Action");
  btn_send_wpt = new PushButton(action_wptGroupBox->NewRow(), "Send wpt");
  btn_clear_wpt = new PushButton(action_wptGroupBox->LastRowLastCol(), "Clear wpt");
  btn_loop = new PushButton(action_wptGroupBox->LastRowLastCol(), "Loop");

  

  // Put some initial values in the labels
  label_mission_wpt_list->SetText("3/3\n5/3.5\n");
  label_entrance_wpt_list->SetText("1.5/2.5\n6/6.5\n");
  label_exit_wpt_list->SetText("7/7.5\n2/5\n");

  // cmb_box_mission_wpt = new ComboBox(settingsTab->NewRow(), "Mission wpt list");
  // cmb_box_mission_wpt->AddItem("7 - 7");
  // cmb_box_mission_wpt->AddItem("2.5 - 3");
  // cmb_box_mission_wpt->AddItem("3 - 6.5");

  mavlinkSocket = new Socket((Thread *)this, "mavlink socket", "127.0.0.1:5000");
  
  recvSocket = new Socket((Thread *)this, "recv socket", "127.0.0.1:5001");

}

Mavlink::~Mavlink() {
}

void Mavlink::Run() {
  Thread::Info("Debug: enter mavlink acquisition loop\n");

  // double latValue;
  // double lonValue;

  Message *msgControllerAction = new Message(1024);

  if (getFrameworkManager()->ErrorOccured()) {
    SafeStop();
    Thread::Err("An error occurred, we don't launch the Run loop.\n");
  }

  while (!ToBeStopped()) {
    // Thread::Info("Debug: Mavlink acquisition loop\n");
    Thread::SleepMS(1000);
    // latValue = latField->Value();
    // lonValue = lonField->Value();
        
        // Check the values of the buttons
        // First tab
        if (btn_initialize->Clicked()) {
                MissionInitialize();
    }
        if (btn_start_mission->Clicked()) {
                MissionStart();
    }
        if (btn_stop_mission->Clicked()) {
                MissionStop();
    }
        if (btn_kill->Clicked()) {
                MissionKill();
    }
        // Second tab
        if (btn_add_mission_wpt->Clicked()) {
                MissionWptAdd();
    }
        if (btn_add_entrance_wpt->Clicked()) {
                EntranceWptAdd();
    }
        if (btn_add_exit_wpt->Clicked()) {
                ExitWptAdd();
    }   
    if (btn_send_wpt->Clicked()) {
                WptSend();
    }
        if (btn_clear_wpt->Clicked()) {
                WptClear();
    }
        if (btn_loop->Clicked()) {
                WptLoopSend();
    }

    // Thread::Info("Debug: Lat=%f, Lon=%f\n", latValue, lonValue);
  }
}

void Mavlink::SendAllWptMessage() {
  Thread::Info("Debug: SendAllWptMessage\n");
  // SendWptMessage(latField_wpt0, lonField_wpt0);
  // SendWptMessage(latField_wpt1, lonField_wpt1);
  // SendWptMessage(latField_wpt2, lonField_wpt2);
  // SendWptMessage(latField_wpt3, lonField_wpt3);
}

void Mavlink::SendWptMessage(gui::DoubleSpinBox* latField, gui::DoubleSpinBox* lonField) {

// (uint8_t system_id, uint8_t component_id, mavlink_message_t* msg,
// uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, 
// float param1, float param2, float param3, float param4, float x, float y, float z)


  Thread::Info("Debug: SendWptMessage\n");
  mavlink_message_t msg;

  mavlink_msg_mission_item_pack(100, 200, &msg, 0, 0, 0, 0, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, latField->Value(), lonField->Value(), 5);


  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  uint16_t buf_size = mavlink_msg_to_send_buffer(buf, &msg);

  // //print the buffer
  // Thread::Info("Debug: DecodeWptMessage : u8 message (buf : %d) :\n", buf_size);
  // for(uint8_t index = 0; index < buf_size; index++) {
  //   Thread::Info("%d\n", buf[index]);
  // }

  std::string message_to_send = U8toString(buf, buf_size);

  //send the message
  mavlinkSocket->SendMessage(message_to_send); 

  DecodeWptMessage(message_to_send);

}

void Mavlink::DecodeWptMessage(std::string wpt_to_decode) {

  Thread::Info("Debug: start of decode loop\n");

  mavlink_message_t rcv_msg;
  mavlink_status_t rcv_status;

  float received_x;
  float received_y;

  const char * message_to_decode = wpt_to_decode.c_str();
  uint8_t message_size = wpt_to_decode.size();

  // Thread::Info("Debug: DecodeWptMessage : u8 message (buf : %d) :\n", message_size);

  for (uint8_t index = 0; index < message_size; index++) {

    //Thread::Info("Hexa data before cast : %02x\n", data_buffer.c_str()[index]);

    uint8_t single_data = (uint8_t)message_to_decode[index];
    // Thread::Info("%d\n", single_data);

    // Thread::Info("Hexa data after cast : %02x\n", single_data);

    if(mavlink_parse_char(MAVLINK_COMM_0, single_data, &rcv_msg, &rcv_status)) {
        // Thread::Info("Debug: in the if, before the switch\n");
        switch(rcv_msg.msgid)
        {
            case MAVLINK_MSG_ID_HEARTBEAT :
                 Thread::Info("Debug: heartbeat received !!!\n");
                 break;
            case MAVLINK_MSG_ID_MISSION_ITEM :
                 // Thread::Info("Debug: mission item received !!!\n");
                 
                 received_x = mavlink_msg_mission_item_get_x(&rcv_msg);
                 received_y = mavlink_msg_mission_item_get_y(&rcv_msg);

                 Thread::Info("Debug: received x : %f, y : %f\n", received_x, received_y);
                 break;
            default :
                  Thread::Info("Debug: default, no item decoded...\n");
                  break;
        }
    }
  }
  Thread::Info("Debug: end of decode loop\n");
}

// First tab callbacks
void Mavlink::MissionInitialize() {
        
}
void Mavlink::MissionStart() {
        
}
void Mavlink::MissionStop() {
        
}
void Mavlink::MissionKill() {
        
}
// Second tab callbacks  
void Mavlink::MissionWptAdd() {
        
}
void Mavlink::EntranceWptAdd() {
        
}
void Mavlink::ExitWptAdd() {
        
}
void Mavlink::WptSend() {
        
}
void Mavlink::WptClear() {
        
}
void Mavlink::WptLoopSend() {
        
}

void Mavlink::SendHeartbeat() {
  Thread::Info("Debug: heartbeat pressed !\n");
  mavlink_message_t msg;

  msg.payload64[0] = (uint64_t)36;

  mavlink_msg_heartbeat_pack(100, 200, &msg, MAV_TYPE_GENERIC, MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 0, MAV_STATE_UNINIT);

  uint8_t buf[MAVLINK_MAX_PACKET_LEN];
  uint16_t buf_size = mavlink_msg_to_send_buffer(buf, &msg);

  // std::string data_buffer = U8toString(buf, buf_size);
  // std::string data_buffer(my_chars);

  // Thread::Info("Debug:  buf_size=%d, data_buffer_size=%d\n", buf_size, data_buffer.size());
  // Thread::Info("Debug: send data_buffer=%s\n", data_buffer.c_str());

  //to send the message thanks to udp
  // mavlinkSocket->SendMessage(data_buffer); 

  //decode the content of the heartbeat message
  Thread::Info("Debug: start of decode loop\n");

  mavlink_message_t rcv_msg;
  mavlink_status_t rcv_status;

  // for (uint8_t index = 0; index < data_buffer.size(); index++) {
  for (uint8_t index = 0; index < buf_size; index++) {

    //Thread::Info("Hexa data before cast : %02x\n", data_buffer.c_str()[index]);

    // uint8_t single_data = static_cast<uint8_t>(data_buffer.c_str()[index]);
    uint8_t single_data = buf[index];

    Thread::Info("Hexa data after cast : %02x\n", single_data);

    if(mavlink_parse_char(MAVLINK_COMM_0, single_data, &rcv_msg, &rcv_status)) {
        Thread::Info("Debug: in the if, before the switch\n");
        switch(msg.msgid)
        {
            case MAVLINK_MSG_ID_HEARTBEAT:
            {
                 Thread::Info("Debug: heartbeat received !!!\n");
            }
            break;
            default:
              //Do nothing
              break;
        }
    }
  }
  Thread::Info("Debug: end of decode loop\n");
}

std::string Mavlink::U8toString(const uint8_t* chars, const uint16_t chars_size) {
  // string converted_chars;
  char message[chars_size];

  // Thread::Info("Debug: U8toString size : %d\n", chars_size);

  for (uint16_t index = 0; index < chars_size; index++) {
    message[index] = (char)chars[index];
    // Thread::Info("Debug: U8toString index : %d, get_data : %02X, saved_data : %02x\n", index, (char)chars[index], message[index]);
    // converted_chars.append((const char*)(&chars[index]));

  }

  std::string converted_chars(message, chars_size);

  // Thread::Info("Debug: U8toString string size : %d\n", converted_chars.size());

  return converted_chars; 
}

} // end namespace sensor
} // end namespace flair