zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

deactivated waypoint management

QGC4.4
pixhawk 15 years ago
parent
6dc9b70645
commit
1bf1799943
2 changed files with 71 additions and 65 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 114
      src/uas/UAS.cc
  2. 22
      src/ui/WaypointList.cc

114
src/uas/UAS.cc
Unescape View File

@ -356,17 +356,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
break; break;
case MAVLINK_MSG_ID_WAYPOINT: case MAVLINK_MSG_ID_WAYPOINT:
{ {
mavlink_waypoint_t wp; // mavlink_waypoint_t wp;
mavlink_msg_waypoint_decode(&message, &wp); // mavlink_msg_waypoint_decode(&message, &wp);
// FIXME // emit waypointUpdated(uasId, wp.id, wp.x, wp.y, wp.z, wp.yaw, wp.autocontinue, wp.active);
emit waypointUpdated(uasId, wp.id, wp.x, wp.y, wp.z, wp.yaw, wp.autocontinue, wp.active);
} }
break; break;
case MAVLINK_MSG_ID_WAYPOINT_REACHED: case MAVLINK_MSG_ID_WAYPOINT_REACHED:
{ {
mavlink_waypoint_reached_t wp; // mavlink_waypoint_reached_t wp;
mavlink_msg_waypoint_reached_decode(&message, &wp); // mavlink_msg_waypoint_reached_decode(&message, &wp);
emit waypointReached(this, wp.id); // emit waypointReached(this, wp.id);
} }
break; break;
case MAVLINK_MSG_ID_STATUSTEXT: case MAVLINK_MSG_ID_STATUSTEXT:
@ -556,15 +555,6 @@ int UAS::getCommunicationStatus() const
return commStatus; return commStatus;
} }
void UAS::requestWaypoints()
{
mavlink_message_t msg;
mavlink_msg_waypoint_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 25);
// Send message twice to increase chance of reception
sendMessage(msg);
qDebug() << "UAS Request WPs";
}
void UAS::requestParameters() void UAS::requestParameters()
{ {
mavlink_message_t msg; mavlink_message_t msg;
@ -916,41 +906,63 @@ void UAS::receiveButton(int buttonIndex)
} }
void UAS::requestWaypoints()
{
// mavlink_message_t msg;
// mavlink_msg_waypoint_request_list_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 25);
// // Send message twice to increase chance of reception
// sendMessage(msg);
// qDebug() << "UAS Request WPs";
}
void UAS::setWaypoint(Waypoint* wp) void UAS::setWaypoint(Waypoint* wp)
{ {
mavlink_message_t msg; // mavlink_message_t msg;
mavlink_waypoint_set_t set; // mavlink_waypoint_set_t set;
set.id = wp->id; // set.id = wp->id;
//QString name = wp->name; // //QString name = wp->name;
// FIXME Check if this works properly // // FIXME Check if this works properly
//name.truncate(MAVLINK_MSG_WAYPOINT_SET_FIELD_NAME_LEN); // //name.truncate(MAVLINK_MSG_WAYPOINT_SET_FIELD_NAME_LEN);
//strcpy((char*)set.name, name.toStdString().c_str()); // //strcpy((char*)set.name, name.toStdString().c_str());
set.autocontinue = wp->autocontinue; // set.autocontinue = wp->autocontinue;
set.target_component = 25; // FIXME // set.target_component = 25; // FIXME
set.target_system = uasId; // set.target_system = uasId;
set.active = wp->current; // set.active = wp->current;
set.x = wp->x; // set.x = wp->x;
set.y = wp->y; // set.y = wp->y;
set.z = wp->z; // set.z = wp->z;
set.yaw = wp->yaw; // set.yaw = wp->yaw;
mavlink_msg_waypoint_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &set); // mavlink_msg_waypoint_set_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &set);
// Send message twice to increase chance of reception // // Send message twice to increase chance of reception
sendMessage(msg); // sendMessage(msg);
} }
void UAS::setWaypointActive(int id) void UAS::setWaypointActive(int id)
{ {
mavlink_message_t msg; // mavlink_message_t msg;
mavlink_waypoint_set_active_t active; // mavlink_waypoint_set_active_t active;
active.id = id; // active.id = id;
active.target_system = uasId; // active.target_system = uasId;
active.target_component = 25; // FIXME // active.target_component = 25; // FIXME
mavlink_msg_waypoint_set_active_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &active); // mavlink_msg_waypoint_set_active_encode(mavlink->getSystemId(), mavlink->getComponentId(), &msg, &active);
// Send message twice to increase chance of reception // // Send message twice to increase chance of reception
sendMessage(msg); // sendMessage(msg);
sendMessage(msg); // sendMessage(msg);
// TODO This should be not directly emitted, but rather being fed back from the UAS // // TODO This should be not directly emitted, but rather being fed back from the UAS
emit waypointSelected(getUASID(), id); // emit waypointSelected(getUASID(), id);
}
void UAS::clearWaypointList()
{
// mavlink_message_t msg;
// // FIXME
// mavlink_waypoint_clear_list_t clist;
// clist.target_system = uasId;
// clist.target_component = 25; // FIXME
// mavlink_msg_waypoint_clear_list_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &clist);
// sendMessage(msg);
// qDebug() << "UAS clears Waypoints!";
} }
@ -1174,15 +1186,3 @@ void UAS::stopLowBattAlarm()
lowBattAlarm = false; lowBattAlarm = false;
} }
} }
void UAS::clearWaypointList()
{
mavlink_message_t msg;
// FIXME
mavlink_waypoint_clear_list_t clist;
clist.target_system = uasId;
clist.target_component = 25; // FIXME
mavlink_msg_waypoint_clear_list_encode(MG::SYSTEM::ID, MG::SYSTEM::COMPID, &msg, &clist);
sendMessage(msg);
qDebug() << "UAS clears Waypoints!";
}

22
src/ui/WaypointList.cc
Unescape View File

@ -99,15 +99,16 @@ void WaypointList::setUAS(UASInterface* uas)
void WaypointList::setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current) void WaypointList::setWaypoint(int uasId, int id, double x, double y, double z, double yaw, bool autocontinue, bool current)
{ {
transmitDelay->start(1000);
QString string = "New waypoint";
if (waypointNames.contains(id))
{
string = waypointNames.value(id);
}
if (uasId == this->uas->getUASID()) if (uasId == this->uas->getUASID())
{ {
transmitDelay->start(1000);
QString string = "New waypoint";
if (waypointNames.contains(id))
{
string = waypointNames.value(id);
}
Waypoint* wp = new Waypoint(string, id, x, y, z, yaw, autocontinue, current); Waypoint* wp = new Waypoint(string, id, x, y, z, yaw, autocontinue, current);
addWaypoint(wp); addWaypoint(wp);
} }
@ -152,7 +153,6 @@ void WaypointList::transmit()
m_ui->transmitButton->setEnabled(false); m_ui->transmitButton->setEnabled(false);
emit clearWaypointList(); emit clearWaypointList();
waypointNames.clear(); waypointNames.clear();
emit
for(int i = 0; i < waypoints.size(); i++) for(int i = 0; i < waypoints.size(); i++)
{ {
Waypoint* wp = waypoints[i]; Waypoint* wp = waypoints[i];
@ -186,7 +186,13 @@ void WaypointList::add()
void WaypointList::addWaypoint(Waypoint* wp) void WaypointList::addWaypoint(Waypoint* wp)
{ {
if (waypoints.contains(wp))
{
removeWaypoint(wp);
}
waypoints.insert(wp->id, wp); waypoints.insert(wp->id, wp);
if (!wpViews.contains(wp)) if (!wpViews.contains(wp))
{ {
WaypointView* wpview = new WaypointView(wp, this); WaypointView* wpview = new WaypointView(wp, this);

Write Preview
Loading…
Cancel
Save