Commit d67528e0 authored by LM's avatar LM

Updated to latest MAVLink

parent d950361a
......@@ -44,7 +44,6 @@ This file is part of the QGROUNDCONTROL project
#include "configuration.h"
#include "QGC.h"
#include "QGCCore.h"
#include "MG.h"
#include "MainWindow.h"
#include "GAudioOutput.h"
......
......@@ -17,7 +17,6 @@
#include <QSettings>
#include <QDesktopServices>
//#include "MG.h"
#include "MAVLinkProtocol.h"
#include "UASInterface.h"
#include "UASManager.h"
......@@ -28,7 +27,6 @@
#include "ArduPilotMegaMAV.h"
#include "configuration.h"
#include "LinkManager.h"
//#include "MainWindow.h"
#include "QGCMAVLink.h"
#include "QGCMAVLinkUASFactory.h"
#include "QGC.h"
......
......@@ -716,11 +716,11 @@ void MAVLinkSimulationLink::writeBytes(const char* data, qint64 size)
}
break;
// EXECUTE OPERATOR ACTIONS
case MAVLINK_MSG_ID_ACTION: {
mavlink_action_t action;
mavlink_msg_action_decode(&msg, &action);
case MAVLINK_MSG_ID_COMMAND: {
mavlink_command_t action;
mavlink_msg_command_decode(&msg, &action);
qDebug() << "SIM" << "received action" << action.action << "for system" << action.target;
qDebug() << "SIM" << "received action" << action.command << "for system" << action.target_system;
// FIXME MAVLINKV10PORTINGNEEDED
// switch (action.action) {
......
......@@ -815,11 +815,11 @@ void MAVLinkSimulationWaypointPlanner::mavlink_handler (const mavlink_message_t*
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) qDebug("Waypoint: received message with action %d\n", action.action);
case MAVLINK_MSG_ID_COMMAND: { // special action from ground station
mavlink_command_t action;
mavlink_msg_command_decode(msg, &action);
if(action.target_system == systemid) {
if (verbose) qDebug("Waypoint: received message with action %d\n", action.command);
// switch (action.action) {
// case MAV_ACTION_LAUNCH:
// if (verbose) std::cerr << "Launch received" << std::endl;
......
......@@ -30,6 +30,7 @@ This file is part of the QGROUNDCONTROL project
#ifndef QGCMAVLINK_H
#define QGCMAVLINK_H
#define MAVLINK_NO_DATA
#include <mavlink_types.h>
#include <mavlink.h>
......
......@@ -32,6 +32,7 @@ This file is part of the QGROUNDCONTROL project
#include "QGCCore.h"
#include "MainWindow.h"
#include "configuration.h"
#include "mavlink_data.h"
/* SDL does ugly things to main() */
......
......@@ -65,7 +65,7 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
mavlink_msg_pattern_detected_decode(&message, &detected);
QByteArray b;
b.resize(256);
mavlink_msg_pattern_detected_get_file(&message, (int8_t*)b.data());
mavlink_msg_pattern_detected_get_file(&message, b.data());
b.append('\0');
QString name = QString(b);
if (detected.type == 0)
......
......@@ -809,16 +809,16 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit parameterChanged(uasId, message.compid, value.param_count, value.param_index, parameterName, val);
}
break;
case MAVLINK_MSG_ID_ACTION_ACK:
mavlink_action_ack_t ack;
mavlink_msg_action_ack_decode(&message, &ack);
case MAVLINK_MSG_ID_CMD_ACK:
mavlink_cmd_ack_t ack;
mavlink_msg_cmd_ack_decode(&message, &ack);
if (ack.result == 1)
{
emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed action: %1").arg(ack.action));
emit textMessageReceived(uasId, message.compid, 0, tr("SUCCESS: Executed CMD: %1").arg(ack.cmd));
}
else
{
emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected action: %1").arg(ack.action));
emit textMessageReceived(uasId, message.compid, 0, tr("FAILURE: Rejected CMD: %1").arg(ack.cmd));
}
break;
case MAVLINK_MSG_ID_DEBUG:
......
#include <mavlink.h>
#include <stdio.h>
int main(int argc, char* argv[])
{
uint8_t bitfield = 254; // 11111110
uint8_t mask = 128; // 10000000
uint8_t result = (bitfield & mask);
printf("0x%02x\n", bitfield);
// Transform into network order
generic_32bit bin;
bin.i = 1;
printf("First byte in (little endian) 0x%02x\n", bin.b[0]);
generic_32bit bout;
bout.b[0] = bin.b[3];
bout.b[1] = bin.b[2];
bout.b[2] = bin.b[1];
bout.b[3] = bin.b[0];
printf("Last byte out (big endian) 0x%02x\n", bout.b[3]);
uint8_t n = 5;
printf("Mask is 0x%02x\n", ((uint32_t)(1 << n)) - 1); // = 2^n - 1
int32_t encoded = 2;
uint8_t bits = 2;
uint8_t packet[MAVLINK_MAX_PACKET_LEN];
uint8_t packet_index = 0;
uint8_t bit_index = 0;
put_bitfield_n_by_index(encoded, bits, packet_index, bit_index, &bit_index, packet);
}
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