Skip to content

Q
qgroundcontrol
Commit 77e27a2a authored by pixhawk's avatar pixhawk
Browse files
Options

Improving simulation

parent 94947491
Hide whitespace changes
Inline Side-by-side
Showing with 188 additions and 98 deletions
images/earth.html
View file @ 77e27a2a
......@@ -71,6 +71,11 @@ function init()
google.earth.createInstance("map3d", initCallback, failureCallback);
}
function setFollowEnabled(enable)
{
followEnabled = enable;
}
function setCurrAircraft(id)
......@@ -214,8 +219,8 @@ function initCallback(object)
ge.getWindow().setVisibility(true);
ge.getOptions().setStatusBarVisibility(true);
ge.getOptions().setScaleLegendVisibility(true);
ge.getOptions().setFlyToSpeed(5.0);
//ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT);
//ge.getOptions().setFlyToSpeed(5.0);
ge.getOptions().setFlyToSpeed(ge.SPEED_TELEPORT);
ge.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO);
ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true);
......@@ -233,11 +238,15 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
if (lastLat == 0)
{
lastLat = currLat;
lastLon = lastLon;
lastLon = currLon;
}
currLat = lat;
currLon = lon;
currAlt = alt;
// Interpolate between t-1 and t and set new states
lastLat = lastLat*0.8+currLat*0.2;
lastLon = lastLon*0.8+currLon*0.2;
lastAlt = lastAlt*0.8+currAlt*0.2;
//currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
......@@ -245,11 +254,15 @@ function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
planeOrient.setRoll(((roll/M_PI)+1.0)*360.0);
planeOrient.setTilt(((pitch/M_PI)+1.0)*360.0);
planeOrient.setHeading(((yaw/M_PI)+1.0)*360.0);
currFollowHeading = ((yaw/M_PI)+1.0)*360.0;
planeLoc.setLatitude(lat);
planeLoc.setLongitude(lon);
planeLoc.setAltitude(alt);
planeLoc.setLatitude(lastLat);
planeLoc.setLongitude(lastLon);
planeLoc.setAltitude(lastAlt);
planeModel.setLocation(planeLoc);
if (followEnabled) updateFollowAircraft();
}
}
......@@ -284,10 +297,6 @@ function setCurrentAircraft(id)
function updateFollowAircraft()
{
currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
// Interpolate between t-1 and t and set new states
lastLat = lastLat*0.7+currLat*0.3;
lastLon = lastLon*0.7+currLon*0.3;
lastAlt = lastAlt*0.7+currAlt*0.3;
currView.setLatitude(lastLat);
currView.setLongitude(lastLon);
currView.setAltitude(lastAlt);
......
src/comm/MAVLinkSimulationLink.cc
View file @ 77e27a2a
......@@ -451,6 +451,11 @@ void MAVLinkSimulationLink::mainloop()
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;
// ATTITUDE VEHICLE 2
mavlink_msg_attitude_pack(54, MAV_COMP_ID_IMU, &ret, 0, 0, 0, atan2((y/2)+0.3, (x+0.002)), 0, 0, 0);
sendMAVLinkMessage(&ret);
// // GLOBAL POSITION VEHICLE 3
// mavlink_msg_global_position_int_pack(60, componentId, &ret, (473780.28137103+(x/2+0.002))*1E3, (85489.9892510421+((y*2)+0.3))*1E3, (z+590.0)*1000.0, 0*100.0, 0*100.0, 0*100.0);
// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
......
src/comm/MAVLinkSimulationMAV.cc
View file @ 77e27a2a
......@@ -11,24 +11,24 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
timer25Hz(0),
timer10Hz(0),
timer1Hz(0),
latitude(0.0),
longitude(0.0),
latitude(47.376389),
longitude(8.548056),
altitude(0.0),
x(0.0),
y(0.0),
z(0.0),
x(8.548056),
y(47.376389),
z(550),
roll(0.0),
pitch(0.0),
yaw(0.0),
globalNavigation(true),
firstWP(false),
previousSPX(0.0),
previousSPY(0.0),
previousSPZ(0.0),
previousSPX(8.548056),
previousSPY(47.376389),
previousSPZ(550),
previousSPYaw(0.0),
nextSPX(0.0),
nextSPY(0.0),
nextSPZ(0.0),
nextSPX(8.548056),
nextSPY(47.376389),
nextSPZ(550),
nextSPYaw(0.0)
{
// Please note: The waypoint planner is running
......@@ -46,38 +46,54 @@ void MAVLinkSimulationMAV::mainloop()
// double xNew = // (nextSPX - previousSPX)
if (!firstWP)
{
double xm = (nextSPX - x) * 0.01;
double ym = (nextSPY - y) * 0.01;
double zm = (nextSPZ - z) * 0.1;
x += xm;
y += ym;
z += zm;
//if (xm < 0.001) xm
}
else
{
x = nextSPX;
y = nextSPY;
z = nextSPZ;
firstWP = false;
}
// 1 Hz execution
if (timer1Hz <= 0)
{
mavlink_message_t msg;
mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK);
link->sendMAVLinkMessage(&msg);
planner.handleMessage(msg);
timer1Hz = 50;
}
// 10 Hz execution
if (timer10Hz <= 0)
{
if (!firstWP)
{
double radPer100ms = 0.0002;
double altPer100ms = 0.1;
double xm = (nextSPX - x);
double ym = (nextSPY - y);
double zm = (nextSPZ - z);
float zsign = (zm < 0) ? -1.0f : 1.0f;
//float trueyaw = atan2f(xm, ym);
yaw = yaw*0.9 + 0.1*atan2f(xm, ym);
qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw;
if (sqrt(xm*xm+ym*ym) > 0.0001)
{
x += cos(yaw)*radPer100ms;
y += sin(yaw)*radPer100ms;
z += altPer100ms*zsign;
}
//if (xm < 0.001) xm
}
else
{
x = nextSPX;
y = nextSPY;
z = nextSPZ;
firstWP = false;
qDebug() << "INIT STEP";
}
mavlink_message_t msg;
mavlink_global_position_int_t pos;
pos.alt = z*1000.0;
......@@ -88,6 +104,16 @@ void MAVLinkSimulationMAV::mainloop()
pos.vz = 0;
mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
link->sendMAVLinkMessage(&msg);
planner.handleMessage(msg);
mavlink_attitude_t attitude;
attitude.roll = 0.0f;
attitude.pitch = 0.0f;
attitude.yaw = yaw;
qDebug() << "YAW" << yaw;
mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
link->sendMAVLinkMessage(&msg);
timer10Hz = 5;
}
......@@ -127,9 +153,9 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
//nextSPYaw = sp.yaw;
// Airplane
yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
//yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);
if (!firstWP) firstWP = true;
//if (!firstWP) firstWP = true;
}
//qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ;
}
......
src/comm/MAVLinkSimulationWaypointPlanner.cc
View file @ 77e27a2a
......@@ -437,6 +437,20 @@ float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, f
return -1.f;
}
float MAVLinkSimulationWaypointPlanner::distanceToPoint(uint16_t seq, float x, float y)
{
if (seq < waypoints->size())
{
mavlink_waypoint_t *cur = waypoints->at(seq);
const PxVector3 A(cur->x, cur->y, 0);
const PxVector3 C(x, y, 0);
return (C-A).length();
}
return -1.f;
}
void MAVLinkSimulationWaypointPlanner::handleMessage(const mavlink_message_t& msg)
{
mavlink_handler(&msg);
......@@ -502,6 +516,10 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
if (att.yaw - yaw_tolerance < wp->yaw || wp->yaw < upperBound)
yawReached = true;
}
// FIXME HACK: Ignore yaw:
yawReached = true;
}
}
break;
......@@ -543,6 +561,50 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
break;
}
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
qDebug() << "GOT GLOBAL POS" << "sys:" << msg->sysid << "wpid" << current_active_wp_id << "wpsize" << waypoints->size();
if(msg->sysid == systemid && current_active_wp_id < waypoints->size())
{
mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id);
if(wp->frame == 0)
{
mavlink_global_position_int_t pos;
mavlink_msg_global_position_int_decode(msg, &pos);
float x = static_cast<double>(pos.lon)/1E7;
float y = static_cast<double>(pos.lat)/1E7;
float z = static_cast<double>(pos.alt)/1000;
qDebug() << "Received new position: x:" << x << "| y:" << y << "| z:" << z;
posReached = false;
yawReached = true;
// FIXME big hack for simulation!
//float oneDegreeOfLatMeters = 111131.745f;
float orbit = 0.0001;
// compare current position (given in message) with current waypoint
//float orbit = wp->param1;
// Convert to degrees
float dist;
dist = distanceToPoint(current_active_wp_id, x, y);
if (dist >= 0.f && dist <= orbit && yawReached)
{
posReached = true;
qDebug() << "WP PLANNER: REACHED POSITION";
}
}
}
break;
}
case MAVLINK_MSG_ID_ACTION: // special action from ground station
{
mavlink_action_t action;
......
src/comm/MAVLinkSimulationWaypointPlanner.h
View file @ 77e27a2a
......@@ -68,6 +68,7 @@ protected:
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);
float distanceToPoint(uint16_t seq, float x, float y);
void mavlink_handler(const mavlink_message_t* msg);
};
......
src/uas/UASWaypointManager.cc
View file @ 77e27a2a
......@@ -138,9 +138,9 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
if(wp->seq == current_wp_id)
{
qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->yaw << wp->autocontinue << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action;
qDebug() << "Got WP: " << wp->seq << wp->x << wp->y << wp->z << wp->yaw << "auto:" << wp->autocontinue << "curr:" << wp->current << wp->param1 << wp->param2 << (MAV_FRAME) wp->frame << (MAV_ACTION) wp->action;
Waypoint *lwp = new Waypoint(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2, (MAV_FRAME) wp->frame, (MAV_ACTION) wp->action);
addWaypoint(lwp);
addWaypoint(lwp, false);
//get next waypoint
current_wp_id++;
......@@ -320,11 +320,15 @@ int UASWaypointManager::setCurrentWaypoint(quint16 seq)
return -1;
}
void UASWaypointManager::addWaypoint(Waypoint *wp)
/**
* @param enforceFirstActive Enforces that the first waypoint is set as active
*/
void UASWaypointManager::addWaypoint(Waypoint *wp, bool enforceFirstActive)
{
if (wp)
{
wp->setId(waypoints.size());
if (enforceFirstActive && waypoints.size() == 0) wp->setCurrent(true);
waypoints.insert(waypoints.size(), wp);
connect(wp, SIGNAL(changed(Waypoint*)), this, SLOT(notifyOfChange(Waypoint*)));
......@@ -517,6 +521,7 @@ void UASWaypointManager::writeWaypoints()
{
if (current_state == WP_IDLE)
{
// Send clear all if count == 0
if (waypoints.count() > 0)
{
protocol_timer.start(PROTOCOL_TIMEOUT_MS);
......@@ -529,7 +534,9 @@ void UASWaypointManager::writeWaypoints()
current_partner_compid = MAV_COMP_ID_WAYPOINTPLANNER;
//clear local buffer
//TODO: Why not replace with waypoint_buffer.clear() ? - because this will lead to memory leaks, the waypoint-structs have to be deleted, clear() would only delete the pointers.
// Why not replace with waypoint_buffer.clear() ?
// because this will lead to memory leaks, the waypoint-structs
// have to be deleted, clear() would only delete the pointers.
while(!waypoint_buffer.empty())
{
delete waypoint_buffer.back();
......
src/uas/UASWaypointManager.h
View file @ 77e27a2a
......@@ -114,7 +114,7 @@ public slots:
void timeout(); ///< Called by the timer if a response times out. Handles send retries.
/** @name Waypoint list operations */
/*@{*/
void addWaypoint(Waypoint *wp); ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
void addWaypoint(Waypoint *wp, bool enforceFirstActive=true); ///< adds a new waypoint to the end of the list and changes its sequence number accordingly
int removeWaypoint(quint16 seq); ///< locally remove the specified waypoint from the storage
void moveWaypoint(quint16 cur_seq, quint16 new_seq); ///< locally move a waypoint from its current position cur_seq to a new position new_seq
void saveWaypoints(const QString &saveFile); ///< saves the local waypoint list to saveFile
......
src/ui/MapWidget.cc
View file @ 77e27a2a
......@@ -94,9 +94,9 @@ MapWidget::MapWidget(QWidget *parent) :
// SET INITIAL POSITION AND ZOOM
// Set default zoom level
mc->setZoom(17);
mc->setZoom(16);
// Zurich, ETH
mc->setView(QPointF(8.548056,47.376389));
mc->setView(QPointF(8.548056,47.376889));
// Veracruz Mexico
//mc->setView(QPointF(-96.105208,19.138955));
......@@ -411,7 +411,7 @@ void MapWidget::captureMapClick(const QMouseEvent* event, const QPointF coordina
if (mav)
{
mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y()));
mav->getWaypointManager()->addWaypoint(new Waypoint(mav->getWaypointManager()->getWaypointList().count(), coordinate.x(), coordinate.y(), 0.0f, 0.0f, true));
}
else
{
......@@ -824,7 +824,8 @@ mc->updateRequest(p->boundingBox().toRect());
//mc->updateRequestNew();//(uasTrails.value(uas->getUASID())->boundingBox().toRect());
if (uas->getUASID() == this->mav->getUASID())
{
// Limit the position update rate
quint64 currTime = MG::TIME::getGroundTimeNow();
if (currTime - lastUpdate > 120)
......@@ -842,6 +843,7 @@ mc->updateRequest(p->boundingBox().toRect());
}
}
}
}
/**
* Center the view on this position
......
src/ui/WaypointList.cc
View file @ 77e27a2a
......@@ -507,29 +507,6 @@ void WaypointList::on_clearWPListButton_clicked()
}
}
/** @brief Add a waypoint by mouse click over the map */
void WaypointList::addWaypointMouse(QPointF coordinate)
{
if (uas)
{
const QVector<Waypoint *> &waypoints = uas->getWaypointManager()->getWaypointList();
if (waypoints.size() > 0)
{
Waypoint *last = waypoints.at(waypoints.size()-1);
Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), last->getZ(), last->getYaw(), last->getAutoContinue(), false, last->getOrbit(), last->getHoldTime());
uas->getWaypointManager()->addWaypoint(wp);
}
else
{
Waypoint *wp = new Waypoint(0, coordinate.x(), coordinate.y(), -0.8, 0.0, true, true, 0.15, 2000);
uas->getWaypointManager()->addWaypoint(wp);
}
}
}
/** @brief The MapWidget informs that a waypoint global was changed on the map */
void WaypointList::waypointGlobalChanged(QPointF coordinate, int indexWP)
......
src/ui/WaypointList.h
View file @ 77e27a2a
......@@ -75,10 +75,6 @@ public slots:
/** @brief Add a waypoint at the current MAV position */
void addCurrentPositonWaypoint();
/** @brief Add a waypoint by mouse click over the map */
void addWaypointMouse(QPointF coordinate);
/** @brief it notifies that a global waypoint goes to do created */
//void setIsWPGlobal(bool value, QPointF centerCoordinate);
//Update events
/** @brief sets statusLabel string */
......
src/ui/WaypointView.cc
View file @ 77e27a2a
......@@ -170,13 +170,13 @@ void WaypointView::changedCurrent(int state)
{
if (state == 0)
{
//m_ui->selectedBox->setChecked(true);
//m_ui->selectedBox->setCheckState(Qt::Checked);
//wp->setCurrent(false);
m_ui->selectedBox->setChecked(true);
m_ui->selectedBox->setCheckState(Qt::Checked);
wp->setCurrent(false);
}
else
{
//wp->setCurrent(true);
wp->setCurrent(true);
emit changeCurrentWaypoint(wp->getId()); //the slot changeCurrentWaypoint() in WaypointList sets all other current flags to false
}
}
......@@ -255,8 +255,14 @@ void WaypointView::updateValues()
{
m_ui->yawSpinBox->setValue(wp->getYaw()/M_PI*180.);
}
m_ui->selectedBox->setChecked(wp->getCurrent());
m_ui->autoContinue->setChecked(wp->getAutoContinue());
if (m_ui->selectedBox->isChecked() != wp->getCurrent())
{
m_ui->selectedBox->setChecked(wp->getCurrent());
}
if (m_ui->autoContinue->isChecked() != wp->getAutoContinue())
{
m_ui->autoContinue->setChecked(wp->getAutoContinue());
}
m_ui->idLabel->setText(QString("%1").arg(wp->getId()));
if (m_ui->orbitSpinBox->value() != wp->getOrbit())
{
......
src/ui/map/MAV2DIcon.cc
View file @ 77e27a2a
......@@ -42,17 +42,18 @@ void MAV2DIcon::setPen(QPen* pen)
*/
void MAV2DIcon::setYaw(float yaw)
{
float diff = fabs(yaw - this->yaw);
while (diff > M_PI)
{
diff -= M_PI;
}
//qDebug() << "MAV2Icon" << yaw;
//float diff = fabs(yaw - this->yaw);
// while (diff > M_PI)
// {
// diff -= M_PI;
// }
if (diff > 0.25)
{
// if (diff > 0.25)
// {
this->yaw = yaw;
drawIcon(mypen);
}
// }
}
void MAV2DIcon::drawIcon(QPen* pen)
......
src/ui/map3D/QGCGoogleEarthView.cc
View file @ 77e27a2a
......@@ -185,6 +185,7 @@ void QGCGoogleEarthView::follow(bool follow)
{
ui->followAirplaneCheckbox->setChecked(follow);
followCamera = follow;
if (gEarthInitialized) javaScript(QString("setFollowEnabled(%1)").arg(follow));
}
void QGCGoogleEarthView::goHome()
......@@ -353,6 +354,8 @@ void QGCGoogleEarthView::initializeGoogleEarth()
// Start update timer
updateTimer->start(refreshRateMs);
follow(this->followCamera);
gEarthInitialized = true;
}
}
......@@ -395,11 +398,6 @@ void QGCGoogleEarthView::updateState()
.arg(pitch, 0, 'f', 9)
.arg(yaw, 0, 'f', 9));
}
if (followCamera)
{
javaScript(QString("updateFollowAircraft()"));
}
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment