Ignore:
Timestamp:
Sep 23, 2016, 11:13:25 AM (9 years ago)
Author:
Thomas Fuhrmann
Message:

Update MavlinkUDP files and add pause function

File:
1 edited

Legend:

Unmodified
Added
Removed

  • branches/mavlink/tools/Controller/Mavlink/src/MavlinkUDP.h

    r78 r84  
    3030#include <sys/time.h>
    3131#include "include/common/mavlink.h"
     32#include "CallbackTimer.h"
    3233
    3334// TODO these constants should be read from a file
    … …  
    4849
    4950#define ACK_TIMEOUT 100000 // us
    50 
    51 #ifndef MAV_CUSTOM_PARAM
    52 #define MAV_CUSTOM_PARAM
    53 
    54 enum TMAVCustomParams {
    55     MAV_LOOP_MISSION_ITEMS=42,
    56     MAV_CUSTOM_PARAM1=43,
    57     MAV_CUSTOM_PARAM2=44
    58 };
    59 
    60 #endif
     51#define HEARTBEAT_DEFAULT_PERIOD 1000000
     52#define POSITION_DEFAULT_PERIOD 1000000
     53#define ATTITUDE_DEFAULT_PERIOD 1000000
     54#define STATUS_DEFAULT_PERIOD 1000000
     55
     56//#ifndef MAV_CUSTOM_PARAM
     57//#define MAV_CUSTOM_PARAM
     58
     59//enum TMAVCustomParams {
     60//    MAV_LOOP_MISSION_ITEMS=42,
     61//    MAV_CUSTOM_PARAM1=43,
     62//    MAV_CUSTOM_PARAM2=44
     63//};
     64
     65//#endif
    6166
    6267#ifndef REBOOT_SHUTDOWN_PARAMS
    … …  
    9499    uint64_t local_position_ned;
    95100    uint64_t global_position_int;
     101    uint64_t set_position_target_local_ned;
     102    uint64_t set_position_target_global_int;
    96103    uint64_t position_target_local_ned;
    97104    uint64_t position_target_global_int;
    … …  
    118125        local_position_ned = 0;
    119126        global_position_int = 0;
     127        set_position_target_local_ned = 0;
     128        set_position_target_global_int = 0;
    120129        position_target_local_ned = 0;
    121130        position_target_global_int = 0;
    … …  
    131140    int sysid;
    132141    int compid;
    133     // system time
    134142    mavlink_system_time_t system_time;
    135     // mission_request
    136143    mavlink_mission_request_t mission_request;
    137     // mission request list
    138144    mavlink_mission_request_list_t mission_request_list;
    139     // mission write partial list
    140145    mavlink_mission_write_partial_list_t mission_write_partial_list;
    141     // mission_reached
    142146    mavlink_mission_item_reached_t mission_reached;
    143     // mission_clear_all
    144147    mavlink_mission_clear_all_t clear_all;
    145     // command long
    146148    mavlink_command_long_t command_long;
    147     // command_ack
    148149    mavlink_command_ack_t command_ack;
    149     // timesync
    150150    mavlink_timesync_t timesync;
    151     // mission_count
    152151    mavlink_mission_count_t mission_count;
    153     // mission_ack
    154152    mavlink_mission_ack_t mission_ack;
    155     // mission_item
    156153    mavlink_mission_item_t mission_item;
    157     // Homeposition
    158154    mavlink_home_position_t home_position;
    159     // Heartbeat
    160155    mavlink_heartbeat_t heartbeat;
    161     // System Status
    162156    mavlink_sys_status_t sys_status;
    163     // Battery Status
    164157    mavlink_battery_status_t battery_status;
    165     // Radio Status
    166158    mavlink_radio_status_t radio_status;
    167     // Local Position
    168159    mavlink_local_position_ned_t local_position_ned;
    169     // Global Position
    170160    mavlink_global_position_int_t global_position_int;
    171     // Local Position Target
     161    mavlink_set_position_target_local_ned_t set_position_target_local_ned;
     162    mavlink_set_position_target_global_int_t set_position_target_global_int;
    172163    mavlink_position_target_local_ned_t position_target_local_ned;
    173     // Global Position Target
    174164    mavlink_position_target_global_int_t position_target_global_int;
    175     // HiRes IMU
    176165    mavlink_highres_imu_t highres_imu;
    177     // Attitude
    178166    mavlink_attitude_t attitude;
    179     // System Parameters?
    180     // To add
    181     // Time Stamps
    182167    Time_Stamps time_stamps;
    183168    void reset_timestamps() {
    … …  
    193178// * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
    194179
    195 struct MissionItem {
    196     uint16_t itemID;
    197     uint8_t type;
    198     uint64_t holdTime;
    199     float radius;
    200     float yawAngle;
    201     float latitude;
    202     float longitude;
    203     float altitude;
    204     float x, y, z;
    205     float abortAltitude;
    206     uint16_t sequence;
    207     uint16_t repeatCount;
    208     MissionItem() {
    209         // struct construtor resets all the values
    210         itemID = sequence = repeatCount = 0;
    211         holdTime = 0;
    212         radius = yawAngle = latitude = longitude = altitude = abortAltitude = x = y = z = 0;
    213     }
    214 };
    215 
    216180// * Commands queue:
    217181// * 176  MAV_CMD_DO_SET_MODE             (mode MAV_MODE)
    218 // * 177  MAV_CMD_DO_JUMP                 (sequence, repeat count)
    219182// * 179  MAV_CMD_DO_SET_HOME             (use current, lat, long, altitude)
    220183// * 193  MAV_CMD_DO_PAUSE_CONTINUE       (holdContinue: 0=hold, 1=continue)
    … …  
    223186// * 20   MAV_CMD_NAV_RETURN_TO_LAUNCH    (empty)
    224187
    225 struct MissionCommand {
    226     uint16_t commandId;
    227     // DO_JUMP
     188struct MavlinkItem {
     189    // TODO add timestamp
     190    //
     191    uint16_t id;
     192    uint8_t target_system;
     193    uint8_t target_component;
    228194    uint16_t sequence;
    229     uint16_t repeatCount;
    230     // DO_SET_HOME
    231     uint8_t useCurrent;
    232     float latitude, longitude, altitude;
     195    uint16_t jump_sequence;
     196    uint16_t jump_repeat_count;
     197    uint8_t use_current;
     198    uint64_t hold_time;
     199    float radius;
     200    float latitude, longitude, altitude, relative_alt;
    233201    float x, y, z;
    234     // DO_PAUSE_CONTINUE
    235     uint8_t pauseContinue;
    236     // MISSION_START
    237     uint16_t firstItem, lastItem;
    238     // DO_SET_MODE
     202    float vx, vy, vz;
     203    float afx, afy, afz;
     204    float yaw, yaw_rate;
     205    float abort_altitude;
     206    float desired_yaw;
     207    float yaw_angle;
     208    float min_pitch;
     209    uint8_t pause_continue;
     210    uint16_t first_item, last_item;
    239211    uint8_t mode;
    240     MissionCommand() {
    241         // reset all the attributes
    242         pauseContinue = useCurrent = 0;
    243         commandId = sequence = repeatCount = 0;
    244         latitude = longitude = altitude = x = y = z = 0;
     212    uint32_t time_boot_ms;
     213    uint8_t coordinate_frame;
     214    uint16_t type_mask;
     215    int32_t lat_int, lon_int;
     216    bool callback_flag;
     217    uint64_t callback_period;
     218    uint16_t callback_message;
     219    MavlinkItem() {
     220        target_system = target_component = 0;
     221        first_item = last_item = 0;
     222        mode = 0;
     223        pause_continue = use_current = 0;
     224        hold_time = 0;
     225        id = sequence = 0;
     226        jump_sequence = jump_repeat_count = 0;
     227        radius = yaw_angle = latitude = longitude = altitude = relative_alt = x = y = z = 0;
     228        desired_yaw = abort_altitude = 0;
     229        min_pitch = 0;
     230        vx = vy = vz = 0;
     231        afx = afy = afz = 0;
     232        yaw = yaw_rate = 0;
     233        time_boot_ms = 0;
     234        coordinate_frame = 0;
     235        type_mask = 0;
     236        lat_int = lon_int = 0;
     237        callback_flag = false;
     238        callback_message = 0;
     239        callback_period = HEARTBEAT_DEFAULT_PERIOD;
    245240    }
    246241};
    … …  
    282277    ~MavlinkUDP();
    283278
    284     // begin SockUDP
    285279    // threads start and stop
    286280    void startThreads();
    … …  
    297291    socklen_t fromlen;
    298292    void clearData();
    299     // end SockUDP
    300 
    301293
    302294    char control_status;
    … …  
    328320    bool debug_messages;
    329321
     322    // received setpoint values
     323    float recvSetpointX;
     324    float recvSetpointY;
     325
    330326    bool sendingPosition;
    331     uint16_t sendingPositionInterval;
     327    uint64_t sendingPositionInterval;
    332328    bool sendingAttitude;
    333     uint16_t sendingAttitudeInterval;
     329    uint64_t sendingAttitudeInterval;
    334330    bool sendingSystemStatus;
    335     uint16_t sendingSystemStatusInterval;
     331    uint64_t sendingSystemStatusInterval;
    336332    bool sendingHeartbeat;
    337     uint16_t sendingHeartbeatInterval;
     333    uint64_t sendingHeartbeatInterval;
    338334
    339335    // queue of mission items (plan de vol)
    340     std::queue<MissionItem> missionPlan;
    341     //void getMissionPlan(std::vector<Waypoint>& x) { x =  missionPlan; }
    342     const std::queue<MissionItem>& getMissionPlan() const { return missionPlan; }
    343     void clearMissionPlan() { std::queue<MissionItem> empty; std::swap(missionPlan, empty); }
     336    std::queue<MavlinkItem> missionPlan;
     337    const std::queue<MavlinkItem>& getMissionPlan() const { return missionPlan; }
     338    void clearMissionPlan() { std::queue<MavlinkItem> empty; std::swap(missionPlan, empty); }
    344339    uint16_t currentMissionItem;
    345340    uint16_t missionFirst;
    346341    uint16_t missionLast;
    347 
    348     // queue of received navigation commands
    349     std::queue<MissionCommand> missionCommands;
    350     const std::queue<MissionCommand>& getMissionCommands() const { return missionCommands; }
    351     void clearMissionCommands() { std::queue<MissionCommand> empty; std::swap(missionCommands, empty); }
    352 
    353     bool mission_items_loop;
    354     void itemsLoop() { mission_items_loop = true; }
    355     void itemsNoLoop() { mission_items_loop = false; }
    356     bool getItemsLoop() const { return mission_items_loop; }
     342    // queue of received commands
     343    std::queue<MavlinkItem> missionCommands;
     344    const std::queue<MavlinkItem>& getMissionCommands() const { return missionCommands; }
     345    void clearMissionCommands() { std::queue<MavlinkItem> empty; std::swap(missionCommands, empty); }
     346
     347//    bool mission_items_loop;
     348//    void itemsLoop() { mission_items_loop = true; }
     349//    void itemsNoLoop() { mission_items_loop = false; }
     350//    bool getItemsLoop() const { return mission_items_loop; }
    357351
    358352    bool missionActive;
    … …  
    392386    void sendMissionClearAll(uint8_t targetSystem, uint8_t targetComponent);
    393387    void sendSetPositionTargetLocalNED(uint32_t timeBootMs, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate);
     388    void sendPositionTargetLocalNED(uint32_t timeBootMs, uint8_t coordinateFrame, uint16_t typeMask, float x, float y, float z, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate);
    394389    void sendSetPositionTargetGlobalInt(uint32_t time_boot_ms, uint8_t targetSystem, uint8_t targetComponent, uint8_t coordinateFrame, uint16_t typeMask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate);
     390    void sendPositionTargetGlobalInt(uint32_t time_boot_ms, uint8_t coordinateFrame, uint16_t typeMask, int32_t lat_int, int32_t lon_int, float alt, float vx, float vy, float vz, float afx, float afy, float afz, float yaw, float yaw_rate);
    395391
    396392    // mavlink commands
    397393    void cmdSetMessageInterval(uint8_t targetSystem, uint8_t targetComponent, uint8_t messageID, int64_t interval_usec);
    398     void cmdWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude);
    399     void cmdLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude); // land at location
     394    void cmdNavWaypoint(uint8_t targetSystem, uint8_t targetComponent, float holdTime, float proximityRadius, float passRadius, float desiredYaw, float latitude, float longitude, float altitude);
     395    void cmdNavLand(uint8_t targetSystem, uint8_t targetComponent, float abortAlt, float desiredYaw, float latitude, float longitude, float altitude); // land at location
    400396    void cmdDoLandStart(uint8_t targetSystem, uint8_t targetComponent, float latitude, float longitude); // mission command to perform landing
    401     void cmdTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude);
     397    void cmdNavTakeoff(uint8_t targetSystem, uint8_t targetComponent, float desiredPitch, float magnetometerYaw, float latitude, float longitude, float altitude);
    402398    void cmdDoSetMode(uint8_t targetSystem, uint8_t targetComponent, uint8_t mavMode);
    403399    void cmdDoSetHome(uint8_t targetSystem, uint8_t targetComponent, uint8_t useCurrent);
     400    void cmdGetHomePosition(uint8_t targetSystem, uint8_t targetComponent);
    404401    void cmdMissionStart(uint8_t targetSystem, uint8_t targetComponent, uint8_t firstItem, uint8_t lastItem);
    405402    void cmdDoSetParameter(uint8_t targetSystem, uint8_t targetComponent, uint8_t paramNumber, float paramValue);
    406403    void cmdRequestAutopilotCapabilities(uint8_t targetSystem, uint8_t targetComponent);
    407     void cmdReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent);
     404    void cmdNavReturnToLaunch(uint8_t targetSystem, uint8_t targetComponent);
     405    void cmdDoPauseContinue(uint8_t targetSystem, uint8_t targetComponent, uint8_t pauseContinue);
    408406    //void cmdVideoStartCapture(uint8_t targetSystem, uint8_t targetComponent);
    409407    //void cmdVideoStopCapture(uint8_t targetSystem, uint8_t targetComponent);
    … …  
    430428private:
    431429
    432     // begin SockUDP
    433430    int m_socket_in;
    434431    int m_socket_out;
    … …  
    444441    bool stop_recv;
    445442    bool stop_send;
    446     //std::queue<std::string> queue_out;
    447     //Queue<uint8_t> queue_out;
    448     // end SockUDP
    449443
    450444    inline void clearBuffer(uint8_t *buffer, int len);
    … …  
    467461    bool shutdown_flag;
    468462
    469     // TODO change to mission item class
     463    int64_t heartbeat_period;
     464    // callback timers
     465    CallbackTimer timerHeartbeat;
     466    void startHeartbeat(uint64_t period);
     467    void stopHeartbeat();
     468    void callbackHeartbeat();
     469
     470    CallbackTimer timerSystemTime;
     471    void startSystemTime(uint64_t period);
     472    void stopSystemTime();
     473    void callbackSystemTime();
     474
     475    void stopAllCallbackTimers();
     476
     477    // debug
    470478    void debug(std::string debug_msg);
    471479};
Note: See TracChangeset for help on using the changeset viewer.