Commit 29da5425 authored by lm's avatar lm

Adjusted to most recent MAVLink

parent 61dd75dc
......@@ -661,15 +661,17 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
// Iterate through all components, through all parameters and emit them
QMap<QString, float>::iterator i;
// Iterate through all components / subsystems
int j = 0;
for (i = onboardParams.begin(); i != onboardParams.end(); ++i)
{
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value());
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)i.key().toStdString().c_str(), i.value(), onboardParams.size(), j);
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer+=bufferlength;
j++;
}
/*
......@@ -715,7 +717,7 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
onboardParams.insert(key, set.param_value);
// Pack message and get size of encoded byte string
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value);
mavlink_msg_param_value_pack(systemId, componentId, &msg, (int8_t*)key.toStdString().c_str(), set.param_value, onboardParams.size(), 0);
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//add data into datastream
......
......@@ -94,7 +94,7 @@ void UASWaypointManager::handleWaypoint(quint8 systemId, quint8 compId, mavlink_
if(wp->seq == current_wp_id)
{
//update the UI FIXME
emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->orbit, wp->hold_time);
emit waypointUpdated(wp->seq, wp->x, wp->y, wp->z, wp->yaw, wp->autocontinue, wp->current, wp->param1, wp->param2);
//get next waypoint
current_wp_id++;
......@@ -237,8 +237,10 @@ void UASWaypointManager::sendWaypoints()
cur_d->autocontinue = cur_s->getAutoContinue();
cur_d->current = cur_s->getCurrent();
cur_d->orbit = cur_s->getOrbit();
cur_d->hold_time = cur_s->getHoldTime();
cur_d->orbit = 0;
cur_d->orbit_direction = 0;
cur_d->param1 = cur_s->getOrbit();
cur_d->param2 = cur_s->getHoldTime();
cur_d->type = 1; //FIXME
cur_d->seq = i;
cur_d->x = cur_s->getX();
......
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