lm
14 years ago
14 changed files with 1240 additions and 57 deletions
@ -0,0 +1,33 @@
@@ -0,0 +1,33 @@
|
||||
#include <QDebug> |
||||
|
||||
#include "MAVLinkSimulationMAV.h" |
||||
|
||||
MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) : |
||||
QObject(parent), |
||||
link(parent), |
||||
planner(parent, systemid), |
||||
systemid(systemid) |
||||
{ |
||||
connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop())); |
||||
mainloopTimer.start(1000); |
||||
connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); |
||||
mainloop(); |
||||
} |
||||
|
||||
void MAVLinkSimulationMAV::mainloop() |
||||
{ |
||||
mavlink_message_t msg; |
||||
mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_ARDUPILOTMEGA); |
||||
link->sendMAVLinkMessage(&msg); |
||||
} |
||||
|
||||
void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) |
||||
{ |
||||
qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid; |
||||
|
||||
switch(msg.msgid) |
||||
{ |
||||
case MAVLINK_MSG_ID_ATTITUDE: |
||||
break; |
||||
} |
||||
} |
||||
@ -0,0 +1,31 @@
@@ -0,0 +1,31 @@
|
||||
#ifndef MAVLINKSIMULATIONMAV_H |
||||
#define MAVLINKSIMULATIONMAV_H |
||||
|
||||
#include <QObject> |
||||
#include <QTimer> |
||||
|
||||
#include "MAVLinkSimulationLink.h" |
||||
#include "MAVLinkSimulationWaypointPlanner.h" |
||||
|
||||
class MAVLinkSimulationMAV : public QObject |
||||
{ |
||||
Q_OBJECT |
||||
public: |
||||
explicit MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid); |
||||
|
||||
signals: |
||||
|
||||
public slots: |
||||
void mainloop(); |
||||
void handleMessage(const mavlink_message_t& msg); |
||||
|
||||
protected: |
||||
MAVLinkSimulationLink* link; |
||||
MAVLinkSimulationWaypointPlanner planner; |
||||
int systemid; |
||||
QTimer mainloopTimer; |
||||
|
||||
|
||||
}; |
||||
|
||||
#endif // MAVLINKSIMULATIONMAV_H
|
||||
@ -0,0 +1,958 @@
@@ -0,0 +1,958 @@
|
||||
/*======================================================================
|
||||
|
||||
PIXHAWK Micro Air Vehicle Flying Robotics Toolkit |
||||
|
||||
(c) 2009-2011 PIXHAWK PROJECT <http://pixhawk.ethz.ch>
|
||||
|
||||
This file is part of the PIXHAWK project |
||||
|
||||
PIXHAWK is free software: you can redistribute it and/or modify |
||||
it under the terms of the GNU General Public License as published by |
||||
the Free Software Foundation, either version 3 of the License, or |
||||
(at your option) any later version. |
||||
|
||||
PIXHAWK is distributed in the hope that it will be useful, |
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of |
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
||||
GNU General Public License for more details. |
||||
|
||||
You should have received a copy of the GNU General Public License |
||||
along with PIXHAWK. If not, see <http://www.gnu.org/licenses/>.
|
||||
|
||||
========================================================================*/ |
||||
|
||||
/**
|
||||
* @file |
||||
* @brief a program to manage waypoints and exchange them with the ground station |
||||
* |
||||
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> |
||||
* @author Benjamin Knecht <bknecht@student.ethz.ch> |
||||
* @author Christian Schluchter <schluchc@ee.ethz.ch> |
||||
*/ |
||||
|
||||
#include <cmath> |
||||
|
||||
#include "MAVLinkSimulationWaypointPlanner.h" |
||||
#include "QGC.h" |
||||
|
||||
class PxMatrix3x3; |
||||
|
||||
|
||||
/**
|
||||
* @brief Pixhawk 3D vector class, can be cast to a local OpenCV CvMat. |
||||
* |
||||
*/ |
||||
class PxVector3 |
||||
{ |
||||
public: |
||||
/** @brief standard constructor */ |
||||
PxVector3(void) {} |
||||
/** @brief copy constructor */ |
||||
PxVector3(const PxVector3 &v) { for (int i=0; i < 3; i++) { m_vec[i] = v.m_vec[i]; } } |
||||
/** @brief x,y,z constructor */ |
||||
PxVector3(const float _x, const float _y, const float _z) { m_vec[0] = _x; m_vec[1] = _y; m_vec[2] = _z; } |
||||
/** @brief broadcast constructor */ |
||||
PxVector3(const float _f) { for (int i=0; i < 3; i++) { m_vec[i] = _f; } } |
||||
|
||||
private: |
||||
/** @brief private constructor (not used here, for SSE compatibility) */ |
||||
PxVector3(const float (&_vec)[3]) { for (int i=0; i < 3; i++) { m_vec[i] = _vec[i]; } } |
||||
|
||||
public: |
||||
/** @brief assignment operator */ |
||||
void operator= (const PxVector3 &r) { for (int i=0; i < 3; i++) { m_vec[i] = r.m_vec[i]; } } |
||||
/** @brief const element access */ |
||||
float operator[] (const int i) const { return m_vec[i]; } |
||||
/** @brief element access */ |
||||
float &operator[] (const int i) { return m_vec[i]; } |
||||
|
||||
// === arithmetic operators ===
|
||||
/** @brief element-wise negation */ |
||||
friend PxVector3 operator- (const PxVector3 &v) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = -v.m_vec[i]; } return ret; } |
||||
friend PxVector3 operator+ (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } return ret; } |
||||
friend PxVector3 operator- (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } return ret; } |
||||
friend PxVector3 operator* (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } return ret; } |
||||
friend PxVector3 operator/ (const PxVector3 &l, const PxVector3 &r) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } return ret; } |
||||
|
||||
friend void operator+= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } } |
||||
friend void operator-= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } } |
||||
friend void operator*= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } } |
||||
friend void operator/= (PxVector3 &l, const PxVector3 &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } } |
||||
|
||||
friend PxVector3 operator+ (const PxVector3 &l, float f) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + f; } return ret; } |
||||
friend PxVector3 operator- (const PxVector3 &l, float f) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - f; } return ret; } |
||||
friend PxVector3 operator* (const PxVector3 &l, float f) { PxVector3 ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * f; } return ret; } |
||||
friend PxVector3 operator/ (const PxVector3 &l, float f) { PxVector3 ret; float inv = 1.f/f; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * inv; } return ret; } |
||||
|
||||
friend void operator+= (PxVector3 &l, float f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + f; } } |
||||
friend void operator-= (PxVector3 &l, float f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - f; } } |
||||
friend void operator*= (PxVector3 &l, float f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * f; } } |
||||
friend void operator/= (PxVector3 &l, float f) { float inv = 1.f/f; for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * inv; } } |
||||
|
||||
// === vector operators ===
|
||||
/** @brief dot product */ |
||||
float dot(const PxVector3 &v) const { return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; } |
||||
/** @brief length squared of the vector */ |
||||
float lengthSquared(void) const { return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; } |
||||
/** @brief length of the vector */ |
||||
float length(void) const { return sqrt(lengthSquared()); } |
||||
/** @brief cross product */ |
||||
PxVector3 cross(const PxVector3 &v) const { return PxVector3(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); } |
||||
/** @brief normalizes the vector */ |
||||
PxVector3 &normalize(void) { const float l = 1.f / length(); for (int i=0; i < 3; i++) { m_vec[i] *= l; } return *this; } |
||||
|
||||
friend class PxMatrix3x3; |
||||
protected: |
||||
float m_vec[3]; |
||||
}; |
||||
|
||||
/**
|
||||
* @brief Pixhawk 3D vector class in double precision, can be cast to a local OpenCV CvMat. |
||||
* |
||||
*/ |
||||
class PxVector3Double |
||||
{ |
||||
public: |
||||
/** @brief standard constructor */ |
||||
PxVector3Double(void) {} |
||||
/** @brief copy constructor */ |
||||
PxVector3Double(const PxVector3Double &v) { for (int i=0; i < 3; i++) { m_vec[i] = v.m_vec[i]; } } |
||||
/** @brief x,y,z constructor */ |
||||
PxVector3Double(const double _x, const double _y, const double _z) { m_vec[0] = _x; m_vec[1] = _y; m_vec[2] = _z; } |
||||
/** @brief broadcast constructor */ |
||||
PxVector3Double(const double _f) { for (int i=0; i < 3; i++) { m_vec[i] = _f; } } |
||||
|
||||
private: |
||||
/** @brief private constructor (not used here, for SSE compatibility) */ |
||||
PxVector3Double(const double (&_vec)[3]) { for (int i=0; i < 3; i++) { m_vec[i] = _vec[i]; } } |
||||
|
||||
public: |
||||
/** @brief assignment operator */ |
||||
void operator= (const PxVector3Double &r) { for (int i=0; i < 3; i++) { m_vec[i] = r.m_vec[i]; } } |
||||
/** @brief const element access */ |
||||
double operator[] (const int i) const { return m_vec[i]; } |
||||
/** @brief element access */ |
||||
double &operator[] (const int i) { return m_vec[i]; } |
||||
|
||||
// === arithmetic operators ===
|
||||
/** @brief element-wise negation */ |
||||
friend PxVector3Double operator- (const PxVector3Double &v) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = -v.m_vec[i]; } return ret; } |
||||
friend PxVector3Double operator+ (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } return ret; } |
||||
friend PxVector3Double operator- (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } return ret; } |
||||
friend PxVector3Double operator* (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } return ret; } |
||||
friend PxVector3Double operator/ (const PxVector3Double &l, const PxVector3Double &r) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } return ret; } |
||||
|
||||
friend void operator+= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + r.m_vec[i]; } } |
||||
friend void operator-= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - r.m_vec[i]; } } |
||||
friend void operator*= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * r.m_vec[i]; } } |
||||
friend void operator/= (PxVector3Double &l, const PxVector3Double &r) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] / r.m_vec[i]; } } |
||||
|
||||
friend PxVector3Double operator+ (const PxVector3Double &l, double f) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] + f; } return ret; } |
||||
friend PxVector3Double operator- (const PxVector3Double &l, double f) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] - f; } return ret; } |
||||
friend PxVector3Double operator* (const PxVector3Double &l, double f) { PxVector3Double ret; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * f; } return ret; } |
||||
friend PxVector3Double operator/ (const PxVector3Double &l, double f) { PxVector3Double ret; double inv = 1.f/f; for (int i=0; i < 3; i++) { ret.m_vec[i] = l.m_vec[i] * inv; } return ret; } |
||||
|
||||
friend void operator+= (PxVector3Double &l, double f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] + f; } } |
||||
friend void operator-= (PxVector3Double &l, double f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] - f; } } |
||||
friend void operator*= (PxVector3Double &l, double f) { for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * f; } } |
||||
friend void operator/= (PxVector3Double &l, double f) { double inv = 1.f/f; for (int i=0; i < 3; i++) { l.m_vec[i] = l.m_vec[i] * inv; } } |
||||
|
||||
// === vector operators ===
|
||||
/** @brief dot product */ |
||||
double dot(const PxVector3Double &v) const { return m_vec[0]*v.m_vec[0] + m_vec[1]*v.m_vec[1] + m_vec[2]*v.m_vec[2]; } |
||||
/** @brief length squared of the vector */ |
||||
double lengthSquared(void) const { return m_vec[0]*m_vec[0] + m_vec[1]*m_vec[1] + m_vec[2]*m_vec[2]; } |
||||
/** @brief length of the vector */ |
||||
double length(void) const { return sqrt(lengthSquared()); } |
||||
/** @brief cross product */ |
||||
PxVector3Double cross(const PxVector3Double &v) const { return PxVector3Double(m_vec[1]*v.m_vec[2] - m_vec[2]*v.m_vec[1], m_vec[2]*v.m_vec[0] - m_vec[0]*v.m_vec[2], m_vec[0]*v.m_vec[1] - m_vec[1]*v.m_vec[0]); } |
||||
/** @brief normalizes the vector */ |
||||
PxVector3Double &normalize(void) { const double l = 1.f / length(); for (int i=0; i < 3; i++) { m_vec[i] *= l; } return *this; } |
||||
|
||||
friend class PxMatrix3x3; |
||||
protected: |
||||
double m_vec[3]; |
||||
}; |
||||
|
||||
MAVLinkSimulationWaypointPlanner::MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int sysid) : |
||||
QObject(parent), |
||||
link(parent), |
||||
idle(false), |
||||
current_active_wp_id(-1), |
||||
timestamp_lastoutside_orbit(0), |
||||
timestamp_firstinside_orbit(0), |
||||
waypoints(&waypoints1), |
||||
waypoints_receive_buffer(&waypoints2), |
||||
current_state(PX_WPP_IDLE), |
||||
protocol_current_wp_id(0), |
||||
protocol_current_count(0), |
||||
protocol_current_partner_systemid(0), |
||||
protocol_current_partner_compid(0), |
||||
protocol_timestamp_lastaction(0), |
||||
protocol_timeout(1000), |
||||
timestamp_last_send_setpoint(0), |
||||
systemid(sysid), |
||||
compid(MAV_COMP_ID_WAYPOINTPLANNER), |
||||
setpointDelay(10), |
||||
yawTolerance(0.4f), |
||||
verbose(true), |
||||
debug(false), |
||||
silent(false) |
||||
{ |
||||
connect(parent, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); |
||||
} |
||||
|
||||
|
||||
|
||||
/*
|
||||
* @brief Sends an waypoint ack message |
||||
*/ |
||||
void MAVLinkSimulationWaypointPlanner::send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type) |
||||
{ |
||||
mavlink_message_t msg; |
||||
mavlink_waypoint_ack_t wpa; |
||||
|
||||
wpa.target_system = target_systemid; |
||||
wpa.target_component = target_compid; |
||||
wpa.type = type; |
||||
|
||||
mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa); |
||||
link->sendMAVLinkMessage(&msg); |
||||
|
||||
|
||||
|
||||
if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); |
||||
} |
||||
|
||||
/*
|
||||
* @brief Broadcasts the new target waypoint and directs the MAV to fly there |
||||
* |
||||
* This function broadcasts its new active waypoint sequence number and |
||||
* sends a message to the controller, advising it to fly to the coordinates |
||||
* of the waypoint with a given orientation |
||||
* |
||||
* @param seq The waypoint sequence number the MAV should fly to. |
||||
*/ |
||||
void MAVLinkSimulationWaypointPlanner::send_waypoint_current(uint16_t seq) |
||||
{ |
||||
if(seq < waypoints->size()) |
||||
{ |
||||
mavlink_waypoint_t *cur = waypoints->at(seq); |
||||
|
||||
mavlink_message_t msg; |
||||
mavlink_waypoint_current_t wpc; |
||||
|
||||
wpc.seq = cur->seq; |
||||
|
||||
mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc); |
||||
link->sendMAVLinkMessage(&msg); |
||||
|
||||
|
||||
|
||||
if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq); |
||||
} |
||||
} |
||||
|
||||
/*
|
||||
* @brief Directs the MAV to fly to a position |
||||
* |
||||
* Sends a message to the controller, advising it to fly to the coordinates |
||||
* of the waypoint with a given orientation |
||||
* |
||||
* @param seq The waypoint sequence number the MAV should fly to. |
||||
*/ |
||||
void MAVLinkSimulationWaypointPlanner::send_setpoint(uint16_t seq) |
||||
{ |
||||
if(seq < waypoints->size()) |
||||
{ |
||||
mavlink_waypoint_t *cur = waypoints->at(seq); |
||||
|
||||
mavlink_message_t msg; |
||||
mavlink_local_position_setpoint_set_t PControlSetPoint; |
||||
|
||||
// send new set point to local IMU
|
||||
if (cur->frame == 1) |
||||
{ |
||||
PControlSetPoint.target_system = systemid; |
||||
PControlSetPoint.target_component = MAV_COMP_ID_IMU; |
||||
PControlSetPoint.x = cur->x; |
||||
PControlSetPoint.y = cur->y; |
||||
PControlSetPoint.z = cur->z; |
||||
PControlSetPoint.yaw = cur->yaw; |
||||
|
||||
mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); |
||||
link->sendMAVLinkMessage(&msg); |
||||
|
||||
|
||||
} |
||||
else |
||||
{ |
||||
if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); |
||||
} |
||||
|
||||
uint64_t now = QGC::groundTimeUsecs(); |
||||
timestamp_last_send_setpoint = now; |
||||
} |
||||
} |
||||
|
||||
void MAVLinkSimulationWaypointPlanner::send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) |
||||
{ |
||||
mavlink_message_t msg; |
||||
mavlink_waypoint_count_t wpc; |
||||
|
||||
wpc.target_system = target_systemid; |
||||
wpc.target_component = target_compid; |
||||
wpc.count = count; |
||||
|
||||
mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc); |
||||
link->sendMAVLinkMessage(&msg); |
||||
|
||||
if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); |
||||
|
||||
|
||||
} |
||||
|
||||
void MAVLinkSimulationWaypointPlanner::send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) |
||||
{ |
||||
if (seq < waypoints->size()) |
||||
{ |
||||
mavlink_message_t msg; |
||||
mavlink_waypoint_t *wp = waypoints->at(seq); |
||||
wp->target_system = target_systemid; |
||||
wp->target_component = target_compid; |
||||
|
||||
if (verbose) printf("Sent waypoint %u (%u / %u / %u / %u / %u / %f / %u / %f / %f / %u / %f / %f / %f / %f / %u)\n", wp->seq, wp->target_system, wp->target_component, wp->seq, wp->frame, wp->action, wp->orbit, wp->orbit_direction, wp->param1, wp->param2, wp->current, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue); |
||||
|
||||
mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); |
||||
link->sendMAVLinkMessage(&msg); |
||||
if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); |
||||
|
||||
|
||||
} |
||||
else |
||||
{ |
||||
if (verbose) printf("ERROR: index out of bounds\n"); |
||||
} |
||||
} |
||||
|
||||
void MAVLinkSimulationWaypointPlanner::send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) |
||||
{ |
||||
mavlink_message_t msg; |
||||
mavlink_waypoint_request_t wpr; |
||||
wpr.target_system = target_systemid; |
||||
wpr.target_component = target_compid; |
||||
wpr.seq = seq; |
||||
mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr); |
||||
link->sendMAVLinkMessage(&msg); |
||||
if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); |
||||
|
||||
|
||||
} |
||||
|
||||
/*
|
||||
* @brief emits a message that a waypoint reached |
||||
* |
||||
* This function broadcasts a message that a waypoint is reached. |
||||
* |
||||
* @param seq The waypoint sequence number the MAV has reached. |
||||
*/ |
||||
void MAVLinkSimulationWaypointPlanner::send_waypoint_reached(uint16_t seq) |
||||
{ |
||||
mavlink_message_t msg; |
||||
mavlink_waypoint_reached_t wp_reached; |
||||
|
||||
wp_reached.seq = seq; |
||||
|
||||
mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached); |
||||
link->sendMAVLinkMessage(&msg); |
||||
|
||||
if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); |
||||
|
||||
|
||||
} |
||||
|
||||
float MAVLinkSimulationWaypointPlanner::distanceToSegment(uint16_t seq, float x, float y, float z) |
||||
{ |
||||
if (seq < waypoints->size()) |
||||
{ |
||||
mavlink_waypoint_t *cur = waypoints->at(seq); |
||||
|
||||
const PxVector3 A(cur->x, cur->y, cur->z); |
||||
const PxVector3 C(x, y, z); |
||||
|
||||
// seq not the second last waypoint
|
||||
if ((uint16_t)(seq+1) < waypoints->size()) |
||||
{ |
||||
mavlink_waypoint_t *next = waypoints->at(seq+1); |
||||
const PxVector3 B(next->x, next->y, next->z); |
||||
const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); |
||||
if (r >= 0 && r <= 1) |
||||
{ |
||||
const PxVector3 P(A + r*(B-A)); |
||||
return (P-C).length(); |
||||
} |
||||
else if (r < 0.f) |
||||
{ |
||||
return (C-A).length(); |
||||
} |
||||
else |
||||
{ |
||||
return (C-B).length(); |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
return (C-A).length(); |
||||
} |
||||
} |
||||
return -1.f; |
||||
} |
||||
|
||||
float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y, float z) |
||||
{ |
||||
if (seq < waypoints->size()) |
||||
{ |
||||
mavlink_waypoint_t *cur = waypoints->at(seq); |
||||
|
||||
const PxVector3 A(cur->x, cur->y, cur->z); |
||||
const PxVector3 C(x, y, z); |
||||
|
||||
return (C-A).length(); |
||||
} |
||||
return -1.f; |
||||
} |
||||
|
||||
void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg) |
||||
{ |
||||
mavlink_handler(&msg); |
||||
} |
||||
|
||||
void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t* msg) |
||||
{ |
||||
// Handle param messages
|
||||
// paramClient->handleMAVLinkPacket(msg);
|
||||
|
||||
//check for timed-out operations
|
||||
|
||||
printf("MAV: %d WAYPOINTPLANNER GOT MESSAGE\n", systemid); |
||||
|
||||
uint64_t now = QGC::groundTimeUsecs(); |
||||
if (now-protocol_timestamp_lastaction > protocol_timeout && current_state != PX_WPP_IDLE) |
||||
{ |
||||
if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); |
||||
current_state = PX_WPP_IDLE; |
||||
protocol_current_count = 0; |
||||
protocol_current_partner_systemid = 0; |
||||
protocol_current_partner_compid = 0; |
||||
protocol_current_wp_id = -1; |
||||
|
||||
if(waypoints->size() == 0) |
||||
{ |
||||
current_active_wp_id = -1; |
||||
} |
||||
} |
||||
|
||||
if(now-timestamp_last_send_setpoint > setpointDelay) |
||||
{ |
||||
send_setpoint(current_active_wp_id); |
||||
} |
||||
|
||||
switch(msg->msgid) |
||||
{ |
||||
case MAVLINK_MSG_ID_ATTITUDE: |
||||
{ |
||||
if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) |
||||
{ |
||||
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); |
||||
if(wp->frame == 1) |
||||
{ |
||||
mavlink_attitude_t att; |
||||
mavlink_msg_attitude_decode(msg, &att); |
||||
float yaw_tolerance = yawTolerance; |
||||
//compare current yaw
|
||||
if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) |
||||
{ |
||||
if (att.yaw - yaw_tolerance <= wp->yaw && att.yaw + yaw_tolerance >= wp->yaw) |
||||
yawReached = true; |
||||
} |
||||
else if(att.yaw - yaw_tolerance < 0.0f) |
||||
{ |
||||
float lowerBound = 360.0f + att.yaw - yaw_tolerance; |
||||
if (lowerBound < wp->yaw || wp->yaw < att.yaw + yaw_tolerance) |
||||
yawReached = true; |
||||
} |
||||
else |
||||
{ |
||||
float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; |
||||
if (att.yaw - yaw_tolerance < wp->yaw || wp->yaw < upperBound) |
||||
yawReached = true; |
||||
} |
||||
} |
||||
} |
||||
break; |
||||
} |
||||
|
||||
case MAVLINK_MSG_ID_LOCAL_POSITION: |
||||
{ |
||||
if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) |
||||
{ |
||||
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); |
||||
|
||||
if(wp->frame == 1) |
||||
{ |
||||
mavlink_local_position_t pos; |
||||
mavlink_msg_local_position_decode(msg, &pos); |
||||
if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); |
||||
|
||||
posReached = false; |
||||
|
||||
// compare current position (given in message) with current waypoint
|
||||
float orbit = wp->param1; |
||||
|
||||
float dist; |
||||
if (wp->param2 == 0) |
||||
{ |
||||
dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z); |
||||
} |
||||
else |
||||
{ |
||||
dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z); |
||||
} |
||||
|
||||
if (dist >= 0.f && dist <= orbit && yawReached) |
||||
{ |
||||
posReached = true; |
||||
} |
||||
} |
||||
} |
||||
break; |
||||
} |
||||
|
||||
case MAVLINK_MSG_ID_ACTION: // special action from ground station
|
||||
{ |
||||
mavlink_action_t action; |
||||
mavlink_msg_action_decode(msg, &action); |
||||
if(action.target == systemid) |
||||
{ |
||||
if (verbose) printf("Waypoint: received message with action %d\n", action.action); |
||||
switch (action.action) |
||||
{ |
||||
// case MAV_ACTION_LAUNCH:
|
||||
// if (verbose) std::cerr << "Launch received" << std::endl;
|
||||
// current_active_wp_id = 0;
|
||||
// if (waypoints->size()>0)
|
||||
// {
|
||||
// setActive(waypoints[current_active_wp_id]);
|
||||
// }
|
||||
// else
|
||||
// if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
|
||||
// break;
|
||||
|
||||
// case MAV_ACTION_CONTINUE:
|
||||
// if (verbose) std::c
|
||||
// err << "Continue received" << std::endl;
|
||||
// idle = false;
|
||||
// setActive(waypoints[current_active_wp_id]);
|
||||
// break;
|
||||
|
||||
// case MAV_ACTION_HALT:
|
||||
// if (verbose) std::cerr << "Halt received" << std::endl;
|
||||
// idle = true;
|
||||
// break;
|
||||
|
||||
// default:
|
||||
// if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
|
||||
// break;
|
||||
} |
||||
} |
||||
break; |
||||
} |
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_ACK: |
||||
{ |
||||
mavlink_waypoint_ack_t wpa; |
||||
mavlink_msg_waypoint_ack_decode(msg, &wpa); |
||||
|
||||
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) |
||||
{ |
||||
protocol_timestamp_lastaction = now; |
||||
|
||||
if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) |
||||
{ |
||||
if (protocol_current_wp_id == waypoints->size()-1) |
||||
{ |
||||
if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); |
||||
current_state = PX_WPP_IDLE; |
||||
protocol_current_wp_id = 0; |
||||
} |
||||
} |
||||
} |
||||
break; |
||||
} |
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: |
||||
{ |
||||
mavlink_waypoint_set_current_t wpc; |
||||
mavlink_msg_waypoint_set_current_decode(msg, &wpc); |
||||
|
||||
if(wpc.target_system == systemid && wpc.target_component == compid) |
||||
{ |
||||
protocol_timestamp_lastaction = now; |
||||
|
||||
if (current_state == PX_WPP_IDLE) |
||||
{ |
||||
if (wpc.seq < waypoints->size()) |
||||
{ |
||||
if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); |
||||
current_active_wp_id = wpc.seq; |
||||
uint32_t i; |
||||
for(i = 0; i < waypoints->size(); i++) |
||||
{ |
||||
if (i == current_active_wp_id) |
||||
{ |
||||
waypoints->at(i)->current = true; |
||||
} |
||||
else |
||||
{ |
||||
waypoints->at(i)->current = false; |
||||
} |
||||
} |
||||
if (verbose) printf("New current waypoint %u\n", current_active_wp_id); |
||||
yawReached = false; |
||||
posReached = false; |
||||
send_waypoint_current(current_active_wp_id); |
||||
send_setpoint(current_active_wp_id); |
||||
timestamp_firstinside_orbit = 0; |
||||
} |
||||
else |
||||
{ |
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); |
||||
} |
||||
} |
||||
} |
||||
break; |
||||
} |
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: |
||||
{ |
||||
mavlink_waypoint_request_list_t wprl; |
||||
mavlink_msg_waypoint_request_list_decode(msg, &wprl); |
||||
if(wprl.target_system == systemid && wprl.target_component == compid) |
||||
{ |
||||
protocol_timestamp_lastaction = now; |
||||
|
||||
if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) |
||||
{ |
||||
if (waypoints->size() > 0) |
||||
{ |
||||
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); |
||||
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); |
||||
current_state = PX_WPP_SENDLIST; |
||||
protocol_current_wp_id = 0; |
||||
protocol_current_partner_systemid = msg->sysid; |
||||
protocol_current_partner_compid = msg->compid; |
||||
} |
||||
else |
||||
{ |
||||
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); |
||||
} |
||||
protocol_current_count = waypoints->size(); |
||||
send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); |
||||
} |
||||
else |
||||
{ |
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because not my systemid or compid.\n"); |
||||
} |
||||
break; |
||||
} |
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_REQUEST: |
||||
{ |
||||
mavlink_waypoint_request_t wpr; |
||||
mavlink_msg_waypoint_request_decode(msg, &wpr); |
||||
if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) |
||||
{ |
||||
protocol_timestamp_lastaction = now; |
||||
|
||||
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
|
||||
if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) |
||||
{ |
||||
if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); |
||||
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); |
||||
if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); |
||||
|
||||
current_state = PX_WPP_SENDLIST_SENDWPS; |
||||
protocol_current_wp_id = wpr.seq; |
||||
send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); |
||||
} |
||||
else |
||||
{ |
||||
if (verbose) |
||||
{ |
||||
if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } |
||||
else if (current_state == PX_WPP_SENDLIST) |
||||
{ |
||||
if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); |
||||
} |
||||
else if (current_state == PX_WPP_SENDLIST_SENDWPS) |
||||
{ |
||||
if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); |
||||
else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); |
||||
} |
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); |
||||
} |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
//we we're target but already communicating with someone else
|
||||
if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) |
||||
{ |
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); |
||||
} |
||||
} |
||||
break; |
||||
} |
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_COUNT: |
||||
{ |
||||
mavlink_waypoint_count_t wpc; |
||||
mavlink_msg_waypoint_count_decode(msg, &wpc); |
||||
if(wpc.target_system == systemid && wpc.target_component == compid) |
||||
{ |
||||
protocol_timestamp_lastaction = now; |
||||
|
||||
if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) |
||||
{ |
||||
if (wpc.count > 0) |
||||
{ |
||||
if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); |
||||
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); |
||||
|
||||
current_state = PX_WPP_GETLIST; |
||||
protocol_current_wp_id = 0; |
||||
protocol_current_partner_systemid = msg->sysid; |
||||
protocol_current_partner_compid = msg->compid; |
||||
protocol_current_count = wpc.count; |
||||
|
||||
printf("clearing receive buffer and readying for receiving waypoints\n"); |
||||
while(waypoints_receive_buffer->size() > 0) |
||||
{ |
||||
delete waypoints_receive_buffer->back(); |
||||
waypoints_receive_buffer->pop_back(); |
||||
} |
||||
|
||||
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); |
||||
} |
||||
else |
||||
{ |
||||
if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); |
||||
else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); |
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); |
||||
} |
||||
} |
||||
break; |
||||
} |
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT: |
||||
{ |
||||
mavlink_waypoint_t wp; |
||||
mavlink_msg_waypoint_decode(msg, &wp); |
||||
|
||||
if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) |
||||
{ |
||||
protocol_timestamp_lastaction = now; |
||||
|
||||
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
|
||||
if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) |
||||
{ |
||||
if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); |
||||
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); |
||||
if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); |
||||
|
||||
current_state = PX_WPP_GETLIST_GETWPS; |
||||
protocol_current_wp_id = wp.seq + 1; |
||||
mavlink_waypoint_t* newwp = new mavlink_waypoint_t; |
||||
memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); |
||||
waypoints_receive_buffer->push_back(newwp); |
||||
|
||||
if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) |
||||
{ |
||||
if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); |
||||
|
||||
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); |
||||
|
||||
if (current_active_wp_id > waypoints_receive_buffer->size()-1) |
||||
{ |
||||
current_active_wp_id = waypoints_receive_buffer->size() - 1; |
||||
} |
||||
|
||||
// switch the waypoints list
|
||||
std::vector<mavlink_waypoint_t*>* waypoints_temp = waypoints; |
||||
waypoints = waypoints_receive_buffer; |
||||
waypoints_receive_buffer = waypoints_temp; |
||||
|
||||
//get the new current waypoint
|
||||
uint32_t i; |
||||
for(i = 0; i < waypoints->size(); i++) |
||||
{ |
||||
if (waypoints->at(i)->current == 1) |
||||
{ |
||||
current_active_wp_id = i; |
||||
//if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
|
||||
yawReached = false; |
||||
posReached = false; |
||||
send_waypoint_current(current_active_wp_id); |
||||
send_setpoint(current_active_wp_id); |
||||
timestamp_firstinside_orbit = 0; |
||||
break; |
||||
} |
||||
} |
||||
|
||||
if (i == waypoints->size()) |
||||
{ |
||||
current_active_wp_id = -1; |
||||
yawReached = false; |
||||
posReached = false; |
||||
timestamp_firstinside_orbit = 0; |
||||
} |
||||
|
||||
current_state = PX_WPP_IDLE; |
||||
} |
||||
else |
||||
{ |
||||
send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
if (current_state == PX_WPP_IDLE) |
||||
{ |
||||
//we're done receiving waypoints, answer with ack.
|
||||
send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); |
||||
printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); |
||||
} |
||||
if (verbose) |
||||
{ |
||||
if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } |
||||
else if (current_state == PX_WPP_GETLIST) |
||||
{ |
||||
if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); |
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); |
||||
} |
||||
else if (current_state == PX_WPP_GETLIST_GETWPS) |
||||
{ |
||||
if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); |
||||
else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); |
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); |
||||
} |
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); |
||||
} |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
//we we're target but already communicating with someone else
|
||||
if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) |
||||
{ |
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); |
||||
} |
||||
else if(wp.target_system == systemid && wp.target_component == compid) |
||||
{ |
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); |
||||
} |
||||
} |
||||
break; |
||||
} |
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: |
||||
{ |
||||
mavlink_waypoint_clear_all_t wpca; |
||||
mavlink_msg_waypoint_clear_all_decode(msg, &wpca); |
||||
|
||||
if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) |
||||
{ |
||||
protocol_timestamp_lastaction = now; |
||||
|
||||
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); |
||||
while(waypoints->size() > 0) |
||||
{ |
||||
delete waypoints->back(); |
||||
waypoints->pop_back(); |
||||
} |
||||
current_active_wp_id = -1; |
||||
} |
||||
else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) |
||||
{ |
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); |
||||
} |
||||
break; |
||||
} |
||||
|
||||
default: |
||||
{ |
||||
if (debug) printf("Waypoint: received message of unknown type\n"); |
||||
break; |
||||
} |
||||
} |
||||
|
||||
//check if the current waypoint was reached
|
||||
if ((posReached && /*yawReached &&*/ !idle)) |
||||
{ |
||||
if (current_active_wp_id < waypoints->size()) |
||||
{ |
||||
mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id); |
||||
|
||||
if (timestamp_firstinside_orbit == 0) |
||||
{ |
||||
// Announce that last waypoint was reached
|
||||
if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); |
||||
send_waypoint_reached(cur_wp->seq); |
||||
timestamp_firstinside_orbit = now; |
||||
} |
||||
|
||||
// check if the MAV was long enough inside the waypoint orbit
|
||||
//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
|
||||
if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) |
||||
{ |
||||
if (cur_wp->autocontinue) |
||||
{ |
||||
cur_wp->current = 0; |
||||
if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1) |
||||
{ |
||||
//the last waypoint was reached, if auto continue is
|
||||
//activated restart the waypoint list from the beginning
|
||||
current_active_wp_id = 1; |
||||
} |
||||
else |
||||
{ |
||||
current_active_wp_id++; |
||||
} |
||||
|
||||
// Fly to next waypoint
|
||||
timestamp_firstinside_orbit = 0; |
||||
send_waypoint_current(current_active_wp_id); |
||||
send_setpoint(current_active_wp_id); |
||||
waypoints->at(current_active_wp_id)->current = true; |
||||
posReached = false; |
||||
//yawReached = false;
|
||||
if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
else |
||||
{ |
||||
timestamp_lastoutside_orbit = now; |
||||
} |
||||
} |
||||
@ -0,0 +1,74 @@
@@ -0,0 +1,74 @@
|
||||
#ifndef MAVLINKSIMULATIONWAYPOINTPLANNER_H |
||||
#define MAVLINKSIMULATIONWAYPOINTPLANNER_H |
||||
|
||||
#include <QObject> |
||||
#include <vector> |
||||
|
||||
#include "MAVLinkSimulationLink.h" |
||||
#include "QGCMAVLink.h" |
||||
|
||||
enum PX_WAYPOINTPLANNER_STATES |
||||
{ |
||||
PX_WPP_IDLE = 0, |
||||
PX_WPP_SENDLIST, |
||||
PX_WPP_SENDLIST_SENDWPS, |
||||
PX_WPP_GETLIST, |
||||
PX_WPP_GETLIST_GETWPS, |
||||
PX_WPP_GETLIST_GOTALL |
||||
}; |
||||
|
||||
class MAVLinkSimulationWaypointPlanner : public QObject |
||||
{ |
||||
Q_OBJECT |
||||
public: |
||||
explicit MAVLinkSimulationWaypointPlanner(MAVLinkSimulationLink *parent, int systemid); |
||||
|
||||
signals: |
||||
|
||||
public slots: |
||||
void handleMessage(const mavlink_message_t& msg); |
||||
|
||||
protected: |
||||
MAVLinkSimulationLink* link; |
||||
bool idle; ///< indicates if the system is following the waypoints or is waiting
|
||||
uint16_t current_active_wp_id; ///< id of current waypoint
|
||||
bool yawReached; ///< boolean for yaw attitude reached
|
||||
bool posReached; ///< boolean for position reached
|
||||
uint64_t timestamp_lastoutside_orbit;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
|
||||
uint64_t timestamp_firstinside_orbit;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
|
||||
|
||||
std::vector<mavlink_waypoint_t*> waypoints1; ///< vector1 that holds the waypoints
|
||||
std::vector<mavlink_waypoint_t*> waypoints2; ///< vector2 that holds the waypoints
|
||||
|
||||
std::vector<mavlink_waypoint_t*>* waypoints; ///< pointer to the currently active waypoint vector
|
||||
std::vector<mavlink_waypoint_t*>* waypoints_receive_buffer; ///< pointer to the receive buffer waypoint vector
|
||||
PX_WAYPOINTPLANNER_STATES current_state; |
||||
uint16_t protocol_current_wp_id; |
||||
uint16_t protocol_current_count; |
||||
uint8_t protocol_current_partner_systemid; |
||||
uint8_t protocol_current_partner_compid; |
||||
uint64_t protocol_timestamp_lastaction; |
||||
unsigned int protocol_timeout; |
||||
uint64_t timestamp_last_send_setpoint; |
||||
uint8_t systemid; |
||||
uint8_t compid; |
||||
unsigned int setpointDelay; |
||||
float yawTolerance; |
||||
bool verbose; |
||||
bool debug; |
||||
bool silent; |
||||
|
||||
void send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type); |
||||
void send_waypoint_current(uint16_t seq); |
||||
void send_setpoint(uint16_t seq); |
||||
void send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count); |
||||
void send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); |
||||
void send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq); |
||||
void send_waypoint_reached(uint16_t seq); |
||||
float distanceToSegment(uint16_t seq, float x, float y, float z); |
||||
float distanceToPoint(uint16_t seq, float x, float y, float z); |
||||
void mavlink_handler(const mavlink_message_t* msg); |
||||
|
||||
}; |
||||
|
||||
#endif // MAVLINKSIMULATIONWAYPOINTPLANNER_H
|
||||
Loading…
Reference in new issue