Changeset 82 in flair-src


Ignore:
Timestamp:
Sep 21, 2016, 3:16:16 PM (4 years ago)
Author:
Thomas Fuhrmann
Message:

Add MissionStart? and Initialize functions

Location:
branches/mavlink/tools/Controller/Mavlink/src
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.cpp

    r81 r82  
    77//
    88//  authors:    Thomas Fuhrmann
     9//              Milan Erdelj
    910//              Copyright Heudiasyc UMR UTC/CNRS 7253
    1011//
     
    4041
    4142  mavCom = new MavlinkUDP(outputAddress, outputPort);
    42   if (mavCom) {
    43     // mavCom->startThreads();   
    44   } else {
    45     //TODO handle the error
    46   }
    47  
    4843}
    4944
     
    5752void GuiGcs::MissionStart() {
    5853  cout << "MavPlanner GuiGcs MissionStart" << endl;
    59   outputSocket->SendMessage("MavPlanner GuiGcs MissionStart");
     54
     55  Initialize();
     56
     57    cout << "--------------------\n";
     58    cout << "   MISSION START\n";
     59    cout << "--------------------\n";
     60
     61    // set auto armed mode
     62    mavCom->cmdDoSetMode(mavCom->target.getSysID(), mavCom->target.getCompID(), MAV_MODE_AUTO_ARMED);
     63    mavCom->waitCommandAck(ACK_TIMEOUT);
     64
     65    usleep(INTER_COMMAND_PAUSE);
     66
     67    // mission start message
     68    mavCom->cmdMissionStart(mavCom->target.getSysID(), mavCom->target.getCompID(), mavCom->missionFirst, mavCom->missionLast);
     69    mavCom->waitCommandAck(ACK_TIMEOUT);
     70
     71    mavCom->missionStarted();
    6072}
    6173
     
    7284void GuiGcs::MissionSend() {
    7385  cout << "MavPlanner GuiGcs MissionSend" << endl;
    74 
    7586
    7687  // upload a flight plan
     
    150161  return returnValue;
    151162}
     163
     164void GuiGcs::Initialize() {
     165    cout << "--------------------\n";
     166    cout << "Initialization START\n";
     167    cout << "--------------------\n";
     168
     169    // UAV start heartbeat, message ID=0, interval 1000 ms
     170    mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_HEARTBEAT, PERIOD_SECOND);
     171    mavCom->waitCommandAck(ACK_TIMEOUT);
     172
     173    usleep(INTER_COMMAND_PAUSE);
     174
     175    // ask for the autopilot capabilities
     176    mavCom->cmdRequestAutopilotCapabilities(mavCom->target.getSysID(), mavCom->target.getCompID());
     177    // TODO wait for autopilot version
     178    //
     179
     180    usleep(INTER_COMMAND_PAUSE);
     181
     182    // initiate system status callback on the UAV, message ID=1, interval 1000 ms
     183    mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_SYS_STATUS, PERIOD_SECOND);
     184
     185    usleep(INTER_COMMAND_PAUSE);
     186
     187    // initiate system time callback on the UAV
     188    mavCom->cmdSetMessageInterval(mavCom->target.getSysID(), mavCom->target.getCompID(), MAVLINK_MSG_ID_SYSTEM_TIME, PERIOD_SECOND);
     189
     190    usleep(INTER_COMMAND_PAUSE);
     191
     192    // set preflight mode
     193    mavCom->cmdDoSetMode(mavCom->target.getSysID(), mavCom->target.getCompID(), MAV_MODE_PREFLIGHT);
     194    mavCom->waitCommandAck(ACK_TIMEOUT);
     195
     196    usleep(INTER_COMMAND_PAUSE);
     197
     198    // set home position
     199    mavCom->cmdDoSetHome(mavCom->target.getSysID(), mavCom->target.getCompID(), 0);
     200    mavCom->waitCommandAck(ACK_TIMEOUT);
     201
     202    usleep(INTER_COMMAND_PAUSE);
     203
     204    // clear all mission waypoints
     205    mavCom->sendMissionClearAll(mavCom->target.getSysID(), mavCom->target.getCompID());
     206    usleep(INTER_COMMAND_PAUSE);
     207    mavCom->waitMissionAck(ACK_TIMEOUT);
     208    cout << "--------------------\n";
     209    cout << "Initialization END\n";
     210    cout << "--------------------\n";
     211}
  • branches/mavlink/tools/Controller/Mavlink/src/GuiGcs.h

    r79 r82  
    77//
    88//  authors:    Thomas Fuhrmann
     9//              Milan Erdelj
    910//              Copyright Heudiasyc UMR UTC/CNRS 7253
    1011//
     
    2425
    2526#include "MavlinkUDP.h"
     27
     28#define INTER_COMMAND_PAUSE 10000 // us
     29#define PERIOD_SECOND 1000000 // us
    2630
    2731namespace flair {
     
    5458
    5559  MavlinkUDP* mavCom;
     60
     61  void Initialize();
    5662};
    5763
Note: See TracChangeset for help on using the changeset viewer.