Commit fc3c73b2 authored by Lorenz Meier's avatar Lorenz Meier

Updated MAVLink, fixed minor issues with contributed style updates, ready for prime time

parent bd7beb1c
......@@ -111,24 +111,34 @@ QGCToolBar QLabel#toolBarBatteryBar {
}
QGCToolBar QLabel#toolBarTimeoutLabel {
color: #000;
color: #FFFFFF;
background-color: #FF0037;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarSafetyLabel {
/* color is for this label defined within the code */
font-size: 15pt;
}
QGCToolBar QLabel#toolBarModeLabel {
color: #ACEBFE;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarStateLabel {
color: #FEC654;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarBatteryVoltageLabel {
color: #0F0;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarWpLabel {
color: #ACEBFE;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarMessageLabel {
......
......@@ -111,24 +111,34 @@ QGCToolBar QLabel#toolBarBatteryBar {
}
QGCToolBar QLabel#toolBarTimeoutLabel {
color: #000;
color: #FFFFFF;
background-color: #FF0037;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarSafetyLabel {
/* color is for this label defined within the code */
font-size: 15pt;
}
QGCToolBar QLabel#toolBarModeLabel {
color: #475E66;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarStateLabel {
color: #80632A;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarBatteryVoltageLabel {
color: #008000;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarWpLabel {
color: #475E66;
font-size: 15pt;
}
QGCToolBar QLabel#toolBarMessageLabel {
......
......@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Jul 1 09:49:34 2013"
#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:17 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
......
......@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Jul 1 09:49:48 2013"
#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:37 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
......
......@@ -3346,25 +3346,28 @@ static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavl
uint16_t i;
mavlink_hil_state_t packet_in = {
93372036854775807ULL,
{ 73.0, 74.0, 75.0, 76.0 },
73.0,
101.0,
129.0,
157.0,
185.0,
213.0,
241.0,
963499128,
963499336,
963499544,
963499752,
19523,
19627,
19731,
19835,
19939,
20043,
20147,
20251,
20355,
20459,
};
mavlink_hil_state_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.time_usec = packet_in.time_usec;
packet1.roll = packet_in.roll;
packet1.pitch = packet_in.pitch;
packet1.yaw = packet_in.yaw;
packet1.rollspeed = packet_in.rollspeed;
packet1.pitchspeed = packet_in.pitchspeed;
packet1.yawspeed = packet_in.yawspeed;
......@@ -3374,13 +3377,10 @@ static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavl
packet1.vx = packet_in.vx;
packet1.vy = packet_in.vy;
packet1.vz = packet_in.vz;
packet1.ind_airspeed = packet_in.ind_airspeed;
packet1.true_airspeed = packet_in.true_airspeed;
packet1.xacc = packet_in.xacc;
packet1.yacc = packet_in.yacc;
packet1.zacc = packet_in.zacc;
mav_array_memcpy(packet1.attitude_quaternion, packet_in.attitude_quaternion, sizeof(float)*4);
memset(&packet2, 0, sizeof(packet2));
......@@ -3389,12 +3389,12 @@ static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavl
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_hil_state_pack(system_id, component_id, &msg , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc );
mavlink_msg_hil_state_pack(system_id, component_id, &msg , packet1.time_usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc );
mavlink_msg_hil_state_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_hil_state_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc );
mavlink_msg_hil_state_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc );
mavlink_msg_hil_state_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
......@@ -3407,7 +3407,7 @@ static void mavlink_test_hil_state(uint8_t system_id, uint8_t component_id, mavl
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_hil_state_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc );
mavlink_msg_hil_state_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.roll , packet1.pitch , packet1.yaw , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.xacc , packet1.yacc , packet1.zacc );
mavlink_msg_hil_state_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
......@@ -4415,6 +4415,79 @@ static void mavlink_test_hil_optical_flow(uint8_t system_id, uint8_t component_i
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_hil_state_quaternion(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
uint16_t i;
mavlink_hil_state_quaternion_t packet_in = {
93372036854775807ULL,
{ 73.0, 74.0, 75.0, 76.0 },
185.0,
213.0,
241.0,
963499336,
963499544,
963499752,
19731,
19835,
19939,
20043,
20147,
20251,
20355,
20459,
};
mavlink_hil_state_quaternion_t packet1, packet2;
memset(&packet1, 0, sizeof(packet1));
packet1.time_usec = packet_in.time_usec;
packet1.rollspeed = packet_in.rollspeed;
packet1.pitchspeed = packet_in.pitchspeed;
packet1.yawspeed = packet_in.yawspeed;
packet1.lat = packet_in.lat;
packet1.lon = packet_in.lon;
packet1.alt = packet_in.alt;
packet1.vx = packet_in.vx;
packet1.vy = packet_in.vy;
packet1.vz = packet_in.vz;
packet1.ind_airspeed = packet_in.ind_airspeed;
packet1.true_airspeed = packet_in.true_airspeed;
packet1.xacc = packet_in.xacc;
packet1.yacc = packet_in.yacc;
packet1.zacc = packet_in.zacc;
mav_array_memcpy(packet1.attitude_quaternion, packet_in.attitude_quaternion, sizeof(float)*4);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_hil_state_quaternion_encode(system_id, component_id, &msg, &packet1);
mavlink_msg_hil_state_quaternion_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_hil_state_quaternion_pack(system_id, component_id, &msg , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc );
mavlink_msg_hil_state_quaternion_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_hil_state_quaternion_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc );
mavlink_msg_hil_state_quaternion_decode(&msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_to_send_buffer(buffer, &msg);
for (i=0; i<mavlink_msg_get_send_buffer_length(&msg); i++) {
comm_send_ch(MAVLINK_COMM_0, buffer[i]);
}
mavlink_msg_hil_state_quaternion_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
memset(&packet2, 0, sizeof(packet2));
mavlink_msg_hil_state_quaternion_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.attitude_quaternion , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.lat , packet1.lon , packet1.alt , packet1.vx , packet1.vy , packet1.vz , packet1.ind_airspeed , packet1.true_airspeed , packet1.xacc , packet1.yacc , packet1.zacc );
mavlink_msg_hil_state_quaternion_decode(last_msg, &packet2);
MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0);
}
static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg)
{
mavlink_message_t msg;
......@@ -4957,6 +5030,7 @@ static void mavlink_test_common(uint8_t system_id, uint8_t component_id, mavlink
mavlink_test_file_transfer_res(system_id, component_id, last_msg);
mavlink_test_hil_gps(system_id, component_id, last_msg);
mavlink_test_hil_optical_flow(system_id, component_id, last_msg);
mavlink_test_hil_state_quaternion(system_id, component_id, last_msg);
mavlink_test_battery_status(system_id, component_id, last_msg);
mavlink_test_setpoint_8dof(system_id, component_id, last_msg);
mavlink_test_setpoint_6dof(system_id, component_id, last_msg);
......
......@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Jul 1 09:51:07 2013"
#define MAVLINK_BUILD_DATE "Thu Jul 4 13:47:40 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
......
......@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Jul 1 09:50:15 2013"
#define MAVLINK_BUILD_DATE "Thu Jul 4 13:11:50 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
......
#ifndef _MAVLINK_CONVERSIONS_H_
#define _MAVLINK_CONVERSIONS_H_
#include <math.h>
/**
* @file mavlink_conversions.h
*
* These conversion functions follow the NASA rotation standards definition file
* available online.
*
* Their intent is to lower the barrier for MAVLink adopters to use gimbal-lock free
* (both rotation matrices, sometimes called DCM, and quaternions are gimbal-lock free)
* rotation representations. Euler angles (roll, pitch, yaw) will be phased out of the
* protocol as widely as possible.
*
* @author James Goppert
*/
MAVLINK_HELPER void mavlink_quaternion_to_dcm(const float quaternion[4], float dcm[3][3])
{
double a = quaternion[0];
double b = quaternion[1];
double c = quaternion[2];
double d = quaternion[3];
double aSq = a * a;
double bSq = b * b;
double cSq = c * c;
double dSq = d * d;
dcm[0][0] = aSq + bSq - cSq - dSq;
dcm[0][1] = 2.0 * (b * c - a * d);
dcm[0][2] = 2.0 * (a * c + b * d);
dcm[1][0] = 2.0 * (b * c + a * d);
dcm[1][1] = aSq - bSq + cSq - dSq;
dcm[1][2] = 2.0 * (c * d - a * b);
dcm[2][0] = 2.0 * (b * d - a * c);
dcm[2][1] = 2.0 * (a * b + c * d);
dcm[2][2] = aSq - bSq - cSq + dSq;
}
MAVLINK_HELPER void mavlink_dcm_to_euler(const float dcm[3][3], float* roll, float* pitch, float* yaw)
{
float phi, theta, psi;
theta = asin(-dcm[2][0]);
if (fabs(theta - M_PI_2) < 1.0e-3f) {
phi = 0.0f;
psi = (atan2(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1]) + phi);
} else if (fabs(theta + M_PI_2) < 1.0e-3f) {
phi = 0.0f;
psi = atan2f(dcm[1][2] - dcm[0][1],
dcm[0][2] + dcm[1][1] - phi);
} else {
phi = atan2f(dcm[2][1], dcm[2][2]);
psi = atan2f(dcm[1][0], dcm[0][0]);
}
*roll = phi;
*pitch = theta;
*yaw = psi;
}
MAVLINK_HELPER void mavlink_quaternion_to_euler(const float quaternion[4], float* roll, float* pitch, float* yaw)
{
float dcm[3][3];
mavlink_quaternion_to_dcm(quaternion, dcm);
mavlink_dcm_to_euler(dcm, roll, pitch, yaw);
}
MAVLINK_HELPER void mavlink_euler_to_quaternion(float roll, float pitch, float yaw, float quaternion[4])
{
double cosPhi_2 = cos((double)roll / 2.0);
double sinPhi_2 = sin((double)roll / 2.0);
double cosTheta_2 = cos((double)pitch / 2.0);
double sinTheta_2 = sin((double)pitch / 2.0);
double cosPsi_2 = cos((double)yaw / 2.0);
double sinPsi_2 = sin((double)yaw / 2.0);
quaternion[0] = (cosPhi_2 * cosTheta_2 * cosPsi_2 +
sinPhi_2 * sinTheta_2 * sinPsi_2);
quaternion[1] = (sinPhi_2 * cosTheta_2 * cosPsi_2 -
cosPhi_2 * sinTheta_2 * sinPsi_2);
quaternion[2] = (cosPhi_2 * sinTheta_2 * cosPsi_2 +
sinPhi_2 * cosTheta_2 * sinPsi_2);
quaternion[3] = (cosPhi_2 * cosTheta_2 * sinPsi_2 -
sinPhi_2 * sinTheta_2 * cosPsi_2);
}
MAVLINK_HELPER void mavlink_dcm_to_quaternion(const float dcm[3][3], float quaternion[4])
{
quaternion[0] = (0.5 * sqrt(1.0 +
(double)(dcm[0][0] + dcm[1][1] + dcm[2][2])));
quaternion[1] = (0.5 * sqrt(1.0 +
(double)(dcm[0][0] - dcm[1][1] - dcm[2][2])));
quaternion[2] = (0.5 * sqrt(1.0 +
(double)(-dcm[0][0] + dcm[1][1] - dcm[2][2])));
quaternion[3] = (0.5 * sqrt(1.0 +
(double)(-dcm[0][0] - dcm[1][1] + dcm[2][2])));
}
MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, float dcm[3][3])
{
double cosPhi = cos(roll);
double sinPhi = sin(roll);
double cosThe = cos(pitch);
double sinThe = sin(pitch);
double cosPsi = cos(yaw);
double sinPsi = sin(yaw);
dcm[0][0] = cosThe * cosPsi;
dcm[0][1] = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
dcm[0][2] = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
dcm[1][0] = cosThe * sinPsi;
dcm[1][1] = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
dcm[1][2] = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
dcm[2][0] = -sinThe;
dcm[2][1] = sinPhi * cosThe;
dcm[2][2] = cosPhi * cosThe;
}
#endif
\ No newline at end of file
......@@ -4,6 +4,7 @@
#include "string.h"
#include "checksum.h"
#include "mavlink_types.h"
#include "mavlink_conversions.h"
#ifndef MAVLINK_HELPER
#define MAVLINK_HELPER
......
......@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Jul 1 09:50:30 2013"
#define MAVLINK_BUILD_DATE "Thu Jul 4 13:12:05 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
......
......@@ -5,7 +5,7 @@
#ifndef MAVLINK_VERSION_H
#define MAVLINK_VERSION_H
#define MAVLINK_BUILD_DATE "Mon Jul 1 09:50:52 2013"
#define MAVLINK_BUILD_DATE "Thu Jul 4 13:12:22 2013"
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 254
......
......@@ -406,10 +406,10 @@ void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
if (systemid == mode.target_system) sys_mode = mode.base_mode;
}
break;
case MAVLINK_MSG_ID_HIL_STATE:
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
{
mavlink_hil_state_t state;
mavlink_msg_hil_state_decode(&msg, &state);
mavlink_hil_state_quaternion_t state;
mavlink_msg_hil_state_quaternion_decode(&msg, &state);
double a = state.attitude_quaternion[0];
double b = state.attitude_quaternion[1];
......
......@@ -3033,7 +3033,7 @@ void UAS::sendHilState(quint64 time_us, float roll, float pitch, float yaw, floa
sinPhi_2 * sinTheta_2 * cosPsi_2);
mavlink_message_t msg;
mavlink_msg_hil_state_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
mavlink_msg_hil_state_quaternion_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg,
time_us, q, rollspeed, pitchspeed, yawspeed,
lat*1e7f, lon*1e7f, alt*1000, vx*100, vy*100, vz*100, ind_airspeed*100, true_airspeed*100, xacc*1000/9.81, yacc*1000/9.81, zacc*1000/9.81);
sendMessage(msg);
......
......@@ -416,18 +416,18 @@ void QGCToolBar::updateView()
// Display the system armed state with a red-on-yellow background if armed or green text if safe.
if (systemArmed)
{
toolBarSafetyLabel->setStyleSheet(QString("QLabel {color: %1; background-color: %2;}").arg(QGC::colorRed.name()).arg(QGC::colorYellow.name()));
toolBarSafetyLabel->setStyleSheet(QString("QLabel {color: %1; background-color: %2; font-size: 15pt;}").arg(QGC::colorRed.name()).arg(QGC::colorYellow.name()));
toolBarSafetyLabel->setText(tr("ARMED"));
}
else
{
if (MainWindow::instance()->getStyle() == MainWindow::QGC_MAINWINDOW_STYLE_LIGHT)
{
toolBarSafetyLabel->setStyleSheet("QLabel {color: #0D820D;}");
toolBarSafetyLabel->setStyleSheet("QLabel {color: #0D820D; font-size: 15pt;}");
}
else
{
toolBarSafetyLabel->setStyleSheet("QLabel {color: #14C814;}");
toolBarSafetyLabel->setStyleSheet("QLabel {color: #14C814; font-size: 15pt;}");
}
toolBarSafetyLabel->setText(tr("SAFE"));
}
......
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