Browse Source

Merge branch 'v10release' of https://github.com/pixhawk/qgroundcontrol into v10release

QGC4.4
pixhawk 14 years ago
parent
commit
dfadbc7581
  1. 15
      files/mavlink_generator/C/include_v1.0/mavlink_helpers.h
  2. 23
      files/mavlink_generator/C/include_v1.0/mavlink_types.h
  3. 44
      files/mavlink_generator/C/include_v1.0/protocol.h
  4. BIN
      files/mavlink_generator/generator/_ctypes.pyd
  5. BIN
      files/mavlink_generator/generator/_hashlib.pyd
  6. BIN
      files/mavlink_generator/generator/_socket.pyd
  7. BIN
      files/mavlink_generator/generator/_ssl.pyd
  8. BIN
      files/mavlink_generator/generator/bz2.pyd
  9. BIN
      files/mavlink_generator/generator/library.zip
  10. BIN
      files/mavlink_generator/generator/mavgen.exe
  11. BIN
      files/mavlink_generator/generator/pyexpat.pyd
  12. BIN
      files/mavlink_generator/generator/python27.dll
  13. BIN
      files/mavlink_generator/generator/select.pyd
  14. BIN
      files/mavlink_generator/generator/unicodedata.pyd
  15. BIN
      files/mavlink_generator/generator/w9xpopen.exe
  16. 6078
      files/vehicles/SenseSoar_Airplane/senseSoarDummy.dae
  17. 5681
      files/vehicles/sFly_Hexrotor/sfly-hex.dae
  18. BIN
      files/vehicles/sFly_Hexrotor/sfly-hex.skp
  19. 759
      images/earth-singlesystem.html
  20. 181
      images/earth.html
  21. 4
      qgroundcontrol.pri
  22. 30
      qgroundcontrol.pro
  23. 3
      src/QGCCore.cc
  24. 2
      src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc
  25. 35
      src/comm/HexSpinBox.cpp
  26. 25
      src/comm/HexSpinBox.h
  27. 37
      src/comm/MAVLinkSimulationLink.cc
  28. 4
      src/comm/MAVLinkSimulationMAV.cc
  29. 2
      src/comm/QGCFlightGearLink.cc
  30. 2
      src/comm/UDPLink.cc
  31. 2
      src/libs/opmapcontrol/src/core/providerstrings.cpp
  32. 32
      src/libs/opmapcontrol/src/core/pureimagecache.cpp
  33. 1
      src/libs/opmapcontrol/src/mapwidget/waypointlineitem.cpp
  34. 15
      src/uas/PxQuadMAV.cc
  35. 57
      src/uas/UAS.cc
  36. 2
      src/uas/UAS.h
  37. 25
      src/ui/HDDisplay.cc
  38. 5
      src/ui/HSIDisplay.cc
  39. 2
      src/ui/HSIDisplay.h
  40. 27
      src/ui/MAVLinkDecoder.cc
  41. 2
      src/ui/MAVLinkDecoder.h
  42. 121
      src/ui/MainWindow.cc
  43. 2
      src/ui/MainWindow.h
  44. 9
      src/ui/QGCMAVLinkInspector.cc
  45. 16
      src/ui/QGCRemoteControlView.cc
  46. 29
      src/ui/map3D/QGCGoogleEarthView.cc
  47. 189
      src/ui/uas/UASView.cc
  48. 3
      src/ui/uas/UASView.h

15
files/mavlink_generator/C/include_v1.0/mavlink_helpers.h

@ -120,7 +120,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint @@ -120,7 +120,7 @@ MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint
*/
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
{
memcpy(buffer, (uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len);
return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
}
@ -182,7 +182,15 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) @@ -182,7 +182,15 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
*/
MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
{
#if MAVLINK_EXTERNAL_RX_BUFFER
// No m_mavlink_message array defined in function,
// has to be defined externally
#ifndef m_mavlink_message
#error ERROR: IF #define MAVLINK_EXTERNAL_RX_BUFFER IS SET, THE BUFFER HAS TO BE ALLOCATED OUTSIDE OF THIS FUNCTION
#endif
#else
static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NUM_BUFFERS];
#endif
/*
default message crc function. You can override this per-system to
@ -209,6 +217,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa @@ -209,6 +217,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
rxmsg->len = 0;
rxmsg->magic = c;
mavlink_start_checksum(rxmsg);
}
break;
@ -269,7 +278,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa @@ -269,7 +278,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
break;
case MAVLINK_PARSE_STATE_GOT_MSGID:
_MAV_PAYLOAD(rxmsg)[status->packet_idx++] = (char)c;
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
mavlink_update_checksum(rxmsg, c);
if (status->packet_idx == rxmsg->len)
{
@ -296,6 +305,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa @@ -296,6 +305,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
else
{
status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
}
break;
@ -317,6 +327,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa @@ -317,6 +327,7 @@ MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_messa
// Successfully got message
status->msg_received = 1;
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
}
break;

23
files/mavlink_generator/C/include_v1.0/mavlink_types.h

@ -1,6 +1,8 @@ @@ -1,6 +1,8 @@
#ifndef MAVLINK_TYPES_H_
#define MAVLINK_TYPES_H_
#include <inttypes.h>
enum MAV_ACTION
{
MAV_ACTION_HOLD = 0,
@ -66,6 +68,8 @@ typedef struct param_union { @@ -66,6 +68,8 @@ typedef struct param_union {
float param_float;
int32_t param_int32;
uint32_t param_uint32;
uint8_t param_uint8;
uint8_t bytes[4];
};
uint8_t type;
} mavlink_param_union_t;
@ -107,12 +111,12 @@ typedef enum { @@ -107,12 +111,12 @@ typedef enum {
#define MAVLINK_MAX_FIELDS 64
typedef struct __mavlink_field_info {
const char *name; // name of this field
const char *print_format; // printing format hint, or NULL
mavlink_message_type_t type; // type of this field
unsigned array_length; // if non-zero, field is an array
unsigned wire_offset; // offset of each field in the payload
unsigned structure_offset; // offset in a C structure
const char *name; // name of this field
const char *print_format; // printing format hint, or NULL
mavlink_message_type_t type; // type of this field
unsigned int array_length; // if non-zero, field is an array
unsigned int wire_offset; // offset of each field in the payload
unsigned int structure_offset; // offset in a C structure
} mavlink_field_info_t;
// note that in this structure the order of fields is the order
@ -123,11 +127,12 @@ typedef struct __mavlink_message_info { @@ -123,11 +127,12 @@ typedef struct __mavlink_message_info {
mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information
} mavlink_message_info_t;
#define _MAV_PAYLOAD(msg) ((char *)(&(msg)->payload64[0]))
#define _MAV_PAYLOAD(msg) ((const char *)(&(msg)->payload64[0]))
#define _MAV_PAYLOAD_NON_CONST(msg) ((char *)((char *)(&(msg)->payload64[0])))
// checksum is immediately after the payload bytes
#define mavlink_ck_a(msg) *(msg->len + (uint8_t *)_MAV_PAYLOAD(msg))
#define mavlink_ck_b(msg) *((msg->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD(msg))
#define mavlink_ck_a(msg) *(msg->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
#define mavlink_ck_b(msg) *((msg->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
typedef enum {
MAVLINK_COMM_0,

44
files/mavlink_generator/C/include_v1.0/protocol.h

@ -153,17 +153,25 @@ static inline void byte_copy_8(char *dst, const char *src) @@ -153,17 +153,25 @@ static inline void byte_copy_8(char *dst, const char *src)
#define _mav_put_double(buf, wire_offset, b) *(double *)&buf[wire_offset] = b
#endif
/*
like memcpy(), but if src is NULL, do a memset to zero
*/
static void mav_array_memcpy(void *dest, const void *src, size_t n)
{
if (src == NULL) {
memset(dest, 0, n);
} else {
memcpy(dest, src, n);
}
}
/*
* Place a char array into a buffer
*/
static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length)
{
if (b == NULL) {
memset(&buf[wire_offset], 0, array_length);
} else {
memcpy(&buf[wire_offset], b, array_length);
}
mav_array_memcpy(&buf[wire_offset], b, array_length);
}
/*
@ -171,11 +179,8 @@ static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const cha @@ -171,11 +179,8 @@ static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const cha
*/
static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length)
{
if (b == NULL) {
memset(&buf[wire_offset], 0, array_length);
} else {
memcpy(&buf[wire_offset], b, array_length);
}
mav_array_memcpy(&buf[wire_offset], b, array_length);
}
/*
@ -183,11 +188,8 @@ static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const @@ -183,11 +188,8 @@ static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const
*/
static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length)
{
if (b == NULL) {
memset(&buf[wire_offset], 0, array_length);
} else {
memcpy(&buf[wire_offset], b, array_length);
}
mav_array_memcpy(&buf[wire_offset], b, array_length);
}
#if MAVLINK_NEED_BYTE_SWAP
@ -207,11 +209,7 @@ static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, co @@ -207,11 +209,7 @@ static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, co
#define _MAV_PUT_ARRAY(TYPE, V) \
static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \
{ \
if (b == NULL) { \
memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \
} else { \
memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
} \
mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \
}
#endif
@ -224,9 +222,9 @@ _MAV_PUT_ARRAY(int64_t, i64) @@ -224,9 +222,9 @@ _MAV_PUT_ARRAY(int64_t, i64)
_MAV_PUT_ARRAY(float, f)
_MAV_PUT_ARRAY(double, d)
#define _MAV_RETURN_char(msg, wire_offset) _MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_int8_t(msg, wire_offset) (int8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_char(msg, wire_offset) (const char)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_int8_t(msg, wire_offset) (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
#if MAVLINK_NEED_BYTE_SWAP
#define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \

BIN
files/mavlink_generator/generator/_ctypes.pyd

Binary file not shown.

BIN
files/mavlink_generator/generator/_hashlib.pyd

Binary file not shown.

BIN
files/mavlink_generator/generator/_socket.pyd

Binary file not shown.

BIN
files/mavlink_generator/generator/_ssl.pyd

Binary file not shown.

BIN
files/mavlink_generator/generator/bz2.pyd

Binary file not shown.

BIN
files/mavlink_generator/generator/library.zip

Binary file not shown.

BIN
files/mavlink_generator/generator/mavgen.exe

Binary file not shown.

BIN
files/mavlink_generator/generator/pyexpat.pyd

Binary file not shown.

BIN
files/mavlink_generator/generator/python27.dll

Binary file not shown.

BIN
files/mavlink_generator/generator/select.pyd

Binary file not shown.

BIN
files/mavlink_generator/generator/unicodedata.pyd

Binary file not shown.

BIN
files/mavlink_generator/generator/w9xpopen.exe

Binary file not shown.

6078
files/vehicles/SenseSoar_Airplane/senseSoarDummy.dae

File diff suppressed because one or more lines are too long

5681
files/vehicles/sFly_Hexrotor/sfly-hex.dae

File diff suppressed because one or more lines are too long

BIN
files/vehicles/sFly_Hexrotor/sfly-hex.skp

Binary file not shown.

759
images/earth-singlesystem.html

@ -0,0 +1,759 @@ @@ -0,0 +1,759 @@
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html>
<head>
<meta http-equiv="content-type" content="text/html; charset=utf-8" />
<!-- <head> -->
<!-- QGroundControl -->
<title>QGroundControl Google Earth View</title>
<!-- *** Replace the key below below with your own API key, available at http://code.google.com/apis/maps/signup.html *** -->
<script type="text/javascript" src="https://getfirebug.com/firebug-lite-beta.js"></script>
<script type="text/javascript" src="https://www.google.com/jsapi?key=ABQIAAAA5Q6wxQ6lxKS8haLVdUJaqhSjosg_0jiTTs2iXtkDVG0n0If1mBRHzhWw5VqBZX-j4NuzoVpU-UaHVg"></script>
<script type="text/javascript">
google.load("earth", "1", { 'language': 'en'});
var ge = null;
var initialized = false;
var currAircraft = 0;
var followEnabled = false;
var lineAltitudeOffset = 0.5; ///< 0.5 m higher than waypoint, prevents the line from entering the ground
var lastLat = 0;
var lastLon = 0;
var lastAlt = 0;
var currLat = 47.3769;
var currLon = 8.549444;
var currAlt = 470;
var currentCameraLatitude = 0;
var currentCameraLongitude = 0;
var currentCameraAltitude = 0;
var currFollowHeading = 0.0;
var homeLat = 0;
var homeLon = 0;
var homeAlt = 0;
var homeViewRange = 800;
var homeLocation = null;
var homeGroundLevel = 0;
var currViewRange = 50.0; ///<< The current viewing range from this position (in meters)
var currTilt = 40.0; ///<< The tilt angle (in degrees)
var currFollowTilt = 40.0;
var currView = null;
var distanceMode = 0;
var viewMode = 0;
var lastTilt = currTilt;
var lastRoll = 0;
var lastHeading = 0;
var M_PI = 3.14159265;
var planeOrient;
var planeLoc;
var aircraft = [];
var aircraftLocations = [];
var aircraftLastLocations = [];
var attitudes = [];
var locations = [];
var trails = [];
var trailPlacemarks = [];
var trailsVisible = [];
var trailColors = [];
var waypoints = [];
var waypointLines = [];
var waypointLinePlacemarks = [];
var waypointLineColors = [];
//var waypointLines = [];
//var trailPlacemarks[id];
var lineStyle;
// Aircraft class
var planeColor = '6600ffff';
// Enable / disable dragging
var draggingAllowed = true;
// Waypoint interaction flags
var dragInfo = null;
var dragWaypointIndex = "";
var dragWaypointLatitude = 0;
var dragWaypointLongitude = 0;
var dragWaypointAltitude = 0;
var dragWaypointPending = false;
// Waypoint creation flags
var newWaypointLatitude = 0;
var newWaypointLongitude = 0;
var newWaypointAltitude = 0;
var newWaypointPending = false;
var clickMode = 0;
var homePlacemark = null;
// Data / heightmap
var heightMapPlacemark = null;
var heightMapModel = null;
function getGlobal(variable)
{
return variable;
}
function getDraggingAllowed()
{
return draggingAllowed;
}
function setDistanceMode(mode)
{
distanceMode = mode;
}
function setDraggingAllowed(allowed)
{
draggingAllowed = allowed;
}
function sampleCurrentPosition()
{
var thisView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
currentCameraLatitude = thisView.getLatitude();
currentCameraLongitude = thisView.getLongitude();
currentCameraGroundAltitude = ge.getGlobe().getGroundAltitude(currentCameraLatitude, currentCameraLongitude);
}
function enableSetHomeMode()
{
clickMode = 1;
}
function setLookAtLatLon(lat, lon)
{
// Keep the current altitude above ground, just move the position
currView = ge.getView().copyAsLookAt(ge.ALTITUDE_RELATIVE_TO_GROUND);
currView.setLatitude(lat);
currView.setLongitude(lon);
ge.getView().setAbstractView(currView);
}
function setNewWaypointPending(pending)
{
newWaypointPending = pending;
document.getElementById('JScript_newWaypointPending').setAttribute('value',pending);
}
function setDragWaypointPending(pending)
{
dragWaypointPending = pending;
document.getElementById('JScript_dragWaypointPending').setAttribute('value',pending);
}
function isInitialized()
{
return initialized;
}
function init()
{
google.earth.createInstance("map3d", initCallback, failureCallback);
}
function setFollowEnabled(enable)
{
followEnabled = enable;
}
function enableEventListener()
{
// listen for mousedown on the window (look specifically for point placemarks)
google.earth.addEventListener(ge.getWindow(), 'mousedown', function(event)
{
if (clickMode == 1)
{
// Set home mode
dragWaypointIndex = 'HOME';
document.getElementById('JScript_dragWaypointIndex').setAttribute('value',dragWaypointIndex);
dragWaypointLatitude = event.getLatitude();
dragWaypointLongitude = event.getLongitude();
dragWaypointAltitude = ge.getGlobe().getGroundAltitude(dragWaypointLatitude, dragWaypointLongitude);
dragWaypointPending = true;
document.getElementById('JScript_dragWaypointLatitude').setAttribute('value',dragWaypointLatitude);
document.getElementById('JScript_dragWaypointLongitude').setAttribute('value',dragWaypointLongitude);
document.getElementById('JScript_dragWaypointAltitude').setAttribute('value',dragWaypointAltitude);
document.getElementById('JScript_dragWaypointPending').setAttribute('value',true);
setGCSHome(dragWaypointLatitude, dragWaypointLongitude, dragWaypointAltitude);
}
if (event.getTarget().getType() == 'KmlPlacemark' &&
event.getTarget().getGeometry().getType() == 'KmlPoint') {
var placemark = event.getTarget();
event.preventDefault();
if (draggingAllowed)
{
if (clickMode == 0)
{
dragInfo = {
placemark: event.getTarget(),
dragged: false
};
}
}
}
});
// listen for mousemove on the globe
google.earth.addEventListener(ge.getGlobe(), 'mousemove', function(event)
{
if (draggingAllowed && (clickMode == 0))
{
if (dragInfo)
{
event.preventDefault();
var point = dragInfo.placemark.getGeometry();
point.setLatitude(event.getLatitude());
point.setLongitude(event.getLongitude());
dragInfo.dragged = true;
dragWaypointIndex = dragInfo.placemark.getDescription();
document.getElementById('JScript_dragWaypointIndex').setAttribute('value',dragWaypointIndex);
dragWaypointLatitude = event.getLatitude();
dragWaypointLongitude = event.getLongitude();
dragWaypointAltitude = point.getAltitude();
dragWaypointPending = true;
document.getElementById('JScript_dragWaypointLatitude').setAttribute('value',dragWaypointLatitude);
document.getElementById('JScript_dragWaypointLongitude').setAttribute('value',dragWaypointLongitude);
document.getElementById('JScript_dragWaypointAltitude').setAttribute('value',dragWaypointAltitude);
document.getElementById('JScript_dragWaypointPending').setAttribute('value',true);
}
}
});
// listen for mouseup on the window
google.earth.addEventListener(ge.getWindow(), 'mouseup', function(event)
{
if (draggingAllowed && (clickMode == 0))
{
if (dragInfo) {
if (dragInfo.dragged)
{
// if the placemark was dragged, prevent balloons from popping up
event.preventDefault();
// Get drag end location
dragWaypointIndex = dragInfo.placemark.getDescription();
document.getElementById('JScript_dragWaypointIndex').setAttribute('value',dragWaypointIndex);
var point = dragInfo.placemark.getGeometry();
dragWaypointLatitude = event.getLatitude();
dragWaypointLongitude = event.getLongitude();
dragWaypointAltitude = point.getAltitude();
dragWaypointPending = true;
document.getElementById('JScript_dragWaypointLatitude').setAttribute('value',dragWaypointLatitude);
document.getElementById('JScript_dragWaypointLongitude').setAttribute('value',dragWaypointLongitude);
document.getElementById('JScript_dragWaypointAltitude').setAttribute('value',dragWaypointAltitude);
document.getElementById('JScript_dragWaypointPending').setAttribute('value',true);
}
dragInfo = null;
}
}
clickMode = 0;
});
// Listen for wp creation request on the globe
google.earth.addEventListener(ge.getGlobe(), 'dblclick', function(event){
if (draggingAllowed)
{
event.preventDefault();
newWaypointLatitude = event.getLatitude();
newWaypointLongitude = event.getLongitude();
newWaypointAltitude = ge.getGlobe().getGroundAltitude(newWaypointLatitude, newWaypointLongitude);
newWaypointPending = true;
document.getElementById('JScript_newWaypointLatitude').setAttribute('value',newWaypointLatitude);
document.getElementById('JScript_newWaypointLongitude').setAttribute('value',newWaypointLongitude);
document.getElementById('JScript_newWaypointAltitude').setAttribute('value',newWaypointAltitude);
document.getElementById('JScript_newWaypointPending').setAttribute('value',true);
}
});
}
function setCurrAircraft(id)
{
currAircraft = id;
}
function setGCSHome(lat, lon, alt)
{
// Only update if position actually changed
if (lat != homeLat || lon != homeLon || alt != homeAlt)
{
homeLat = lat;
homeLon = lon;
homeAlt = alt;
if (homePlacemark == null)
{
var placemark = ge.createPlacemark('');
var icon = ge.createIcon('');
icon.setHref('http://google-maps-icons.googlecode.com/files/blackH.png');
var style = ge.createStyle('');
style.getIconStyle().setIcon(icon);
//style.getIconStyle().setScale(0.5);
placemark.setStyleSelector(style);
placemark.setDescription('HOME');
// Set the placemark's location.
homeLocation = ge.createPoint('');
homeLocation.setLatitude(lat);
homeLocation.setLongitude(lon);
homeLocation.setAltitude(alt);
placemark.setGeometry(homeLocation);
// Add the placemark to Earth.
ge.getFeatures().appendChild(placemark);
homePlacemark = placemark;
}
else
{
var location = ge.createPoint('');
if (location.getLatitude() != lat || location.getLongitude() != lon || location.getAltitude() != alt)
{
location.setLatitude(lat);
location.setLongitude(lon);
location.setAltitude(alt);
homePlacemark.setGeometry(location);
homePlacemark.setDescription('HOME');
}
}
homeGroundLevel = ge.getGlobe().getGroundAltitude(lat,lon);
if (homeGroundLevel == 0)
{
homeGroundLevel = alt;
}
}
}
function updateWaypointListLength(id, len)
{
// Delete any non-needed waypoints
if (waypoints.length > len)
{
var initialLength = waypoints.length;
for (var i=len; i<initialLength; i++)
{
var placemark = waypoints.pop();
ge.getFeatures().removeChild(placemark);
waypointLines[id].getCoordinates().pop();
}
}
}
function updateWaypoint(id, index, lat, lon, alt, action)
{
// Check if waypoint exists
if (waypoints.length > index)
{
// Waypoint exists
// Set the placemark's location.
var location = ge.createPoint('');
location.setLatitude(lat);
location.setLongitude(lon);
location.setAltitude(alt);
waypoints[index].setGeometry(location);
waypoints[index].setDescription(index+"");
// Set the WP line location
waypointLines[id].getCoordinates().setLatLngAlt(index, lat, lon, alt);
}
else
{
// Waypoint does not exist yet
var placemark = ge.createPlacemark('');
var icon = ge.createIcon('');
var numberstring = index;
if (index < 10) numberstring = '0' + numberstring
icon.setHref('http://google-maps-icons.googlecode.com/files/red' + numberstring +'.png');
var style = ge.createStyle('');
//console.log('WP ICON created:' + 'http://google-maps-icons.googlecode.com/files/red' + numberstring +'.png');
style.getIconStyle().setIcon(icon);
//style.getIconStyle().setScale(0.5);
placemark.setStyleSelector(style);
placemark.setDescription(index+"");
// Set the placemark's location.
var location = ge.createPoint('');
location.setLatitude(lat);
location.setLongitude(lon);
location.setAltitude(alt+lineAltitudeOffset);
placemark.setGeometry(location);
// Add the placemark to Earth.
ge.getFeatures().appendChild(placemark);
waypoints[index] = placemark;
// Add LineString points
waypointLines[id].getCoordinates().pushLatLngAlt(lat, lon, alt);
}
}
function createAircraft(id, type, color)
{
planePlacemark = ge.createPlacemark('');
planePlacemark.setName('aircraft');
planeModel = ge.createModel('');
ge.getFeatures().appendChild(planePlacemark);
planeLoc = ge.createLocation('');
planeModel.setLocation(planeLoc);
planeLink = ge.createLink('');
planeOrient = ge.createOrientation('');
planeModel.setOrientation(planeOrient);
var factor = 1.0;
//planeLink.setHref('http://www.asl.ethz.ch/people/rudink/senseSoarDummy.dae');
planeLink.setHref('http://qgroundcontrol.org/_media/users/models/sfly-hex.dae');
factor = 1.0/1000.0;
//planeLink.setHref('http://qgroundcontrol.org/_media/users/models/ascent-park-glider.dae');
planeModel.setLink(planeLink);
var scale = planeModel.getScale();
scale.set(scale.getX()*factor, scale.getY()*factor, scale.getZ()*factor)
planeModel.setScale(scale);
planeModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE);
planeLoc.setLatitude(currLat);
planeLoc.setLongitude(currLon);
planeLoc.setAltitude(currAlt);
planePlacemark.setGeometry(planeModel);
// Write into global structure
aircraft[id] = planePlacemark;
attitudes[id] = planeOrient;
aircraftLocations[id] = planeLoc;
aircraftLastLocations[id] = ge.createLocation('');
createTrail(id, color);
createWaypointLine(id, color);
}
function createTrail(id, color)
{
trailPlacemarks[id] = ge.createPlacemark('');
// Create the placemark
// Create the LineString; set it to extend down to the ground
// and set the altitude mode
trails[id] = ge.createLineString('');
trailPlacemarks[id].setGeometry(trails[id]);
trails[id].setExtrude(false);
trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
// Create a style and set width and color of line
trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
lineStyle.setWidth(5);
trailColors[id] = color;
lineStyle.getColor().set('00000000');  // aabbggrr format
trailsVisible[id] = false;
}
function createWaypointLine(id, color)
{
// Create the placemark
waypointLinePlacemarks[id] = ge.createPlacemark('');
// Create the LineString; set it to extend down to the ground
// and set the altitude mode
waypointLines[id] = ge.createLineString('');
waypointLinePlacemarks[id].setGeometry(waypointLines[id]);
waypointLines[id].setExtrude(false);
waypointLines[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
// Add LineString points
//lineString.getCoordinates().pushLatLngAlt(48.754, -121.835, 700);
// Create a style and set width and color of line
waypointLinePlacemarks[id].setStyleSelector(ge.createStyle(''));
lineStyle = waypointLinePlacemarks[id].getStyleSelector().getLineStyle();
lineStyle.setWidth(15);
waypointLineColors[id] = color;
lineStyle.getColor().set(color);  // aabbggrr format
// Create a style and set width and color of line
//waypointLinePlacemarks[id].setStyleSelector(ge.createStyle(''));
//lineStyle = waypointLinePlacemarks[id].getStyleSelector().getLineStyle();
//lineStyle.setWidth(15);
//lineStyle.getColor().set(waypointLineColors[id]);  // aabbggrr format
//lineStyle.getColor().set(color);  // aabbggrr format
// Add the feature to Earth
ge.getFeatures().appendChild(waypointLinePlacemarks[id]);
}
function clearTrail(id)
{
ge.getFeatures().removeChild(trailPlacemarks[id]);
trailPlacemarks[id] = null;
var isVisible = trailsVisible[id];
createTrail(id, trailColors[id]);
if (isVisible)
{
showTrail(id);
}
}
function hideTrail(id)
{
trailsVisible[id] = false;
ge.getFeatures().removeChild(trailPlacemarks[id]);
}
function showTrail(id)
{
ge.getFeatures().appendChild(trailPlacemarks[id]);
trailsVisible[id] = true;
}
function setViewRange(dist)
{
currViewRange = dist;
}
function addTrailPosition(id, lat, lon, alt)
{
//trails[id].setExtrude(false);
//trails[id].setAltitudeMode(ge.ALTITUDE_ABSOLUTE);
// Add LineString points
trails[id].getCoordinates().pushLatLngAlt(lat, lon, alt);
// Create a style and set width and color of line
trailPlacemarks[id].setStyleSelector(ge.createStyle(''));
lineStyle = trailPlacemarks[id].getStyleSelector().getLineStyle();
lineStyle.setWidth(5);
lineStyle.getColor().set(trailColors[id]);  // aabbggrr format
}
function initCallback(object)
{
ge = 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.getNavigationControl().setVisibility(ge.VISIBILITY_AUTO);
ge.getLayerRoot().enableLayerById(ge.LAYER_TERRAIN, true);
ge.getLayerRoot().enableLayerById(ge.LAYER_BUILDINGS, true);
ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true);
ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
enableEventListener();
document.getElementById('JScript_initialized').setAttribute('value','true');
// Load heightmap
// http://www.inf.ethz.ch/personal/lomeier/data/untex-environment.dae
heightMapPlacemark = ge.createPlacemark('');
heightMapPlacemark.setName('aircraft');
heightMapModel = ge.createModel('');
ge.getFeatures().appendChild(heightMapPlacemark);
planeLoc = ge.createLocation('');
heightMapModel.setLocation(planeLoc);
planeLink = ge.createLink('');
planeOrient = ge.createOrientation('');
heightMapModel.setOrientation(planeOrient);
planeLink.setHref('http://www.inf.ethz.ch/personal/lomeier/data/untex-environment.dae');
heightMapModel.setLink(planeLink);
var scale = heightMapModel.getScale();
var factor = 1.0;//1.0/1000.0;
scale.set(scale.getX()*factor, scale.getY()*factor, scale.getZ()*factor)
heightMapModel.setScale(scale);
heightMapModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE);
planeLoc.setLatitude(currLat);
planeLoc.setLongitude(currLon);
planeLoc.setAltitude(currAlt);
heightMapPlacemark.setGeometry(heightMapModel);
initialized = true;
}
function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
{
if (id == currAircraft)
{
if (lastLat == 0)
{
lastLat = currLat;
lastLon = currLon;
}
currLat = lat;
currLon = lon;
var trueGroundAlt = ge.getGlobe().getGroundAltitude(lat, lon);
if (trueGroundAlt < alt)
{
currAlt = alt;
}
else
{
currAlt = trueGroundAlt+0.1;
}
// Interpolate between t-1 and t and set new states
lastLat = lastLat*0.5+currLat*0.5;
lastLon = lastLon*0.5+currLon*0.5;
lastAlt = lastAlt*0.5+currAlt*0.5;
planeOrient.setRoll(+((roll/M_PI))*180.0);
planeOrient.setTilt(-((pitch/M_PI))*180.0);
planeOrient.setHeading(((yaw/M_PI))*180.0-90.0);
planeModel.setOrientation(planeOrient);
currFollowHeading = currFollowHeading*0.95+0.05*(((yaw/M_PI))*180.0);
planeLoc.setLatitude(lastLat);
planeLoc.setLongitude(lastLon);
planeLoc.setAltitude(lastAlt);
planeModel.setLocation(planeLoc);
if (followEnabled) updateFollowAircraft();
}
}
function enableDaylight(enabled)
{
if(enabled)
{
ge.getSun().setVisibility(true);
}
else
{
ge.getSun().setVisibility(false);
}
}
function enableAtmosphere(enabled)
{
ge.getOptions().setAtmosphereVisibility(enabled);
}
function goHome()
{
var currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
currView.setLatitude(homeLat);
currView.setLongitude(homeLon);
currView.setAltitude(homeAlt);
currView.setRange(homeViewRange);
currView.setTilt(currTilt);
ge.getView().setAbstractView(currView);
}
function setCurrentAircraft(id)
{
currAircraft = id;
}
/** @brief Set the current view mode
*
* @param mode 0: side map view, 1: top/north map view, 2: fixed chase cam, 3: free chase cam
*/
function setViewMode(mode)
{
var currView = ge.getView().copyAsLookAt(ge.ALTITUDE_RELATIVE_TO_GROUND);
if (mode == 0)
{
currView.setTilt(lastTilt);
currView.setHeading(lastHeading);
}
if (mode == 1 && viewMode != mode)
{
lastTilt = currView.getTilt();
lastHeading = currView.getHeading();
currView.setTilt(0);
currView.setHeading(0);
}
viewMode = mode;
ge.getView().setAbstractView(currView);
}
function updateFollowAircraft()
{
currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
currView.setLatitude(lastLat);
currView.setLongitude(lastLon);
if (distanceMode == 1)
{
var groundAltitude = ge.getGlobe().getGroundAltitude(lastLat, lastLon);
currView.setAltitude(groundAltitude);
}
if (distanceMode == 0) currView.setAltitude(lastAlt);
currView.setRange(currViewRange);
if (viewMode != 3) // VIEW_MODE_CHASE_FREE
{
currView.setTilt(currFollowTilt);
currView.setHeading(currFollowHeading);
}
ge.getView().setAbstractView(currView);
}
function failureCallback(object)
{
}
</script>
<style type="text/css">
html, body {
margin: 0;
width: 100%;
height: 100%;
}
</style>
</head>
<body onload='init()' id='body'>
<center>
<div id='map3d' style='margin: 0; spacing: 0; height: 100%; width: 100%'></div>
</center>
<input type="hidden" id="JScript_initialized" value="false" />
<input type="hidden" id="JScript_dragWaypointIndex" value="0" />
<input type="hidden" id="JScript_dragWaypointLatitude" value="0" />
<input type="hidden" id="JScript_dragWaypointLongitude" value="0" />
<input type="hidden" id="JScript_dragWaypointAltitude" value="0" />
<input type="hidden" id="JScript_dragWaypointPending" value="false" />
<input type="hidden" id="JScript_newWaypointLatitude" value="0" />
<input type="hidden" id="JScript_newWaypointLongitude" value="0" />
<input type="hidden" id="JScript_newWaypointAltitude" value="0" />
<input type="hidden" id="JScript_newWaypointPending" value="false" />
<input type="hidden" id="JScript_currentCameraLatitude" value="0" />
<input type="hidden" id="JScript_currentCameraLongitude" value="0" />
<input type="hidden" id="JScript_currentCameraGroundAltitude" value="0" />
</body>
</html>

181
images/earth.html

@ -1,4 +1,4 @@ @@ -1,4 +1,4 @@
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html>
<head>
@ -7,20 +7,19 @@ @@ -7,20 +7,19 @@
<!-- QGroundControl -->
<title>QGroundControl Google Earth View</title>
<!-- *** Replace the key below below with your own API key, available at http://code.google.com/apis/maps/signup.html *** -->
<!--<script type="text/javascript" src="https://getfirebug.com/firebug-lite-beta.js"></script>-->
<script type="text/javascript" src="http://www.google.com/jsapi?key=ABQIAAAAwbkbZLyhsmTCWXbTcjbgbRSzHs7K5SvaUdm8ua-Xxy_-2dYwMxQMhnagaawTo7L1FE1-amhuQxIlXw"></script>
<script type="text/javascript" src="https://getfirebug.com/firebug-lite-beta.js"></script>
<script type="text/javascript" src="https://www.google.com/jsapi?key=ABQIAAAA5Q6wxQ6lxKS8haLVdUJaqhSjosg_0jiTTs2iXtkDVG0n0If1mBRHzhWw5VqBZX-j4NuzoVpU-UaHVg"></script>
<script type="text/javascript">
google.load("earth", "1", { 'language': 'en'});
var ge = null;
var initialized = false;
var currAircraft = 0;
var followEnabled = false;
var lineAltitudeOffset = 0.5; ///< 0.5 m higher than waypoint, prevents the line from entering the ground
var lastLat = 0;
var lastLon = 0;
var lastAlt = 0;
var lastLat = [];
var lastLon = [];
var lastAlt = [];
var currLat = 47.3769;
var currLon = 8.549444;
var currAlt = 470;
@ -59,6 +58,9 @@ var planeLoc; @@ -59,6 +58,9 @@ var planeLoc;
var aircraft = [];
var aircraftLocations = [];
var aircraftLastLocations = [];
var aircraftScaleFactors = [];
var aircraftLinks = [];
var aircraftModels = [];
var attitudes = [];
var locations = [];
var trails = [];
@ -99,6 +101,11 @@ var clickMode = 0; @@ -99,6 +101,11 @@ var clickMode = 0;
var homePlacemark = null;
// Data / heightmap
var heightMapPlacemark = null;
var heightMapModel = null;
function getGlobal(variable)
{
return variable;
@ -297,7 +304,6 @@ function setGCSHome(lat, lon, alt) @@ -297,7 +304,6 @@ function setGCSHome(lat, lon, alt)
homeLon = lon;
homeAlt = alt;
if (homePlacemark == null)
{
var placemark = ge.createPlacemark('');
@ -407,31 +413,39 @@ function updateWaypoint(id, index, lat, lon, alt, action) @@ -407,31 +413,39 @@ function updateWaypoint(id, index, lat, lon, alt, action)
function createAircraft(id, type, color)
{
planePlacemark = ge.createPlacemark('');
planePlacemark.setName('aircraft');
planeModel = ge.createModel('');
ge.getFeatures().appendChild(planePlacemark);
planeLoc = ge.createLocation('');
planeModel.setLocation(planeLoc);
planeLink = ge.createLink('');
planeOrient = ge.createOrientation('');
planeModel.setOrientation(planeOrient);
planeLink.setHref('http://www.asl.ethz.ch/people/rudink/senseSoarDummy.dae');
planeModel.setLink(planeLink);
planeModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE);
planeLoc.setLatitude(currLat);
planeLoc.setLongitude(currLon);
planeLoc.setAltitude(currAlt);
planePlacemark.setGeometry(planeModel);
aircraft[id] = ge.createPlacemark('');
aircraft[id].setName('aircraft');
aircraftModels[id] = ge.createModel('');
ge.getFeatures().appendChild(aircraft[id]);
aircraftLocations[id] = ge.createLocation('');
aircraftModels[id].setLocation(aircraftLocations[id]);
aircraftLinks[id] = ge.createLink('');
attitudes[id] = ge.createOrientation('');
aircraftModels[id].setOrientation(attitudes[id]);
aircraftScaleFactors[id] = 1.0;
//planeLink.setHref('http://www.asl.ethz.ch/people/rudink/senseSoarDummy.dae');
aircraftLinks[id].setHref('http://qgroundcontrol.org/_media/users/models/sfly-hex.dae');
aircraftScaleFactors[id] = 1.0/1000.0;
//planeLink.setHref('http://qgroundcontrol.org/_media/users/models/ascent-park-glider.dae');
aircraftModels[id].setLink(aircraftLinks[id]);
var scale = aircraftModels[id].getScale();
scale.set(scale.getX()*aircraftScaleFactors[id], scale.getY()*aircraftScaleFactors[id], scale.getZ()*aircraftScaleFactors[id])
aircraftModels[id].setScale(scale);
aircraftModels[id].setAltitudeMode (ge.ALTITUDE_ABSOLUTE);
aircraftLocations[id].setLatitude(currLat);
aircraftLocations[id].setLongitude(currLon);
aircraftLocations[id].setAltitude(currAlt);
aircraft[id].setGeometry(aircraftModels[id]);
// Write into global structure
aircraft[id] = planePlacemark;
attitudes[id] = planeOrient;
aircraftLocations[id] = planeLoc;
aircraftLastLocations[id] = ge.createLocation('');
aircraftLastLocations[id] = aircraftLocations[id];
lastLat[id] = 0;
lastLon[id] = 0;
lastAlt[id] = 0;
createTrail(id, color);
createWaypointLine(id, color);
@ -551,53 +565,83 @@ function initCallback(object) @@ -551,53 +565,83 @@ function initCallback(object)
ge.getLayerRoot().enableLayerById(ge.LAYER_BORDERS, true);
ge.getLayerRoot().enableLayerById(ge.LAYER_TREES, true);
enableEventListener();
document.getElementById('JScript_initialized').setAttribute('value','true');
// Load heightmap
// http://www.inf.ethz.ch/personal/lomeier/data/untex-environment.dae
/*heightMapPlacemark = ge.createPlacemark('');
heightMapPlacemark.setName('aircraft');
heightMapModel = ge.createModel('');
ge.getFeatures().appendChild(heightMapPlacemark);
planeLoc = ge.createLocation('');
heightMapModel.setLocation(planeLoc);
planeLink = ge.createLink('');
planeOrient = ge.createOrientation('');
heightMapModel.setOrientation(planeOrient);
planeLink.setHref('http://www.inf.ethz.ch/personal/lomeier/data/untex-environment.dae');
heightMapModel.setLink(planeLink);
var scale = heightMapModel.getScale();
var factor = 1.0;//1.0/1000.0;
scale.set(scale.getX()*factor, scale.getY()*factor, scale.getZ()*factor)
heightMapModel.setScale(scale);
heightMapModel.setAltitudeMode (ge.ALTITUDE_ABSOLUTE);
planeLoc.setLatitude(currLat);
planeLoc.setLongitude(currLon);
planeLoc.setAltitude(currAlt);
heightMapPlacemark.setGeometry(heightMapModel);*/
enableEventListener();
document.getElementById('JScript_initialized').setAttribute('value','true');
initialized = true;
}
function setAircraftPositionAttitude(id, lat, lon, alt, roll, pitch, yaw)
{
if (lastLat[id] == 0)
{
lastLat[id] = lat;
lastLon[id] = lon;
}
if (id == currAircraft)
{
if (lastLat == 0)
{
lastLat = currLat;
lastLon = currLon;
}
currFollowHeading = currFollowHeading*0.95+0.05*(((yaw/M_PI))*180.0);
currLat = lat;
currLon = lon;
var trueGroundAlt = ge.getGlobe().getGroundAltitude(lat, lon);
if (trueGroundAlt < alt)
{
currAlt = alt;
currAlt = alt;
}
else
{
currAlt = trueGroundAlt+0.1;
currAlt = trueGroundAlt+0.1;
}
// 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;
}
// Interpolate between t-1 and t and set new states
//lastLat[id] = lastLat[id]*0.5+lat*0.5;
//lastLon[id] = lastLon[id]*0.5+lon*0.5;
//lastAlt[id] = lastAlt[id]*0.5+alt*0.5;
planeOrient.setRoll(+((pitch / M_PI)) * 180.0);
planeOrient.setTilt(-((roll / M_PI)) * 180.0);
planeOrient.setHeading(((yaw / M_PI)) * 180.0 - 90.0);
planeModel.setOrientation(planeOrient);
lastLat[id] = lastLat[id]*0.5+lat*0.5;
lastLon[id] = lastLon[id]*0.5+lon*0.5;
lastAlt[id] = lastAlt[id]*0.5+alt*0.5;
currFollowHeading = ((yaw/M_PI))*180.0;
attitudes[id].setRoll(+((roll/M_PI))*180.0);
attitudes[id].setTilt(-((pitch/M_PI))*180.0);
attitudes[id].setHeading(((yaw/M_PI))*180.0-90.0);
aircraftModels[id].setOrientation(attitudes[id]);
planeLoc.setLatitude(lastLat);
planeLoc.setLongitude(lastLon);
planeLoc.setAltitude(lastAlt);
planeModel.setLocation(planeLoc);
aircraftLocations[id].setLatitude(lastLat[id]);
aircraftLocations[id].setLongitude(lastLon[id]);
aircraftLocations[id].setAltitude(lastAlt[id]);
aircraftModels[id].setLocation(aircraftLocations[id]);
if (followEnabled) updateFollowAircraft();
}
if (followEnabled && id == currAircraft) updateFollowAircraft();
}
function enableDaylight(enabled)
@ -663,32 +707,25 @@ function setViewMode(mode) @@ -663,32 +707,25 @@ function setViewMode(mode)
function updateFollowAircraft()
{
currView = ge.getView().copyAsLookAt(ge.ALTITUDE_ABSOLUTE);
currView.setLatitude(lastLat);
currView.setLongitude(lastLon);
currView.setLatitude(lastLat[currAircraft]);
currView.setLongitude(lastLon[currAircraft]);
if (distanceMode == 1)
{
var groundAltitude = ge.getGlobe().getGroundAltitude(lastLat, lastLon);
var groundAltitude = ge.getGlobe().getGroundAltitude(lastLat[currAircraft], lastLon[currAircraft]);
currView.setAltitude(groundAltitude);
}
if (distanceMode == 0) currView.setAltitude(lastAlt);
if (distanceMode == 0) currView.setAltitude(lastAlt[currAircraft]);
currView.setRange(currViewRange);
if (viewMode != 3) // VIEW_MODE_CHASE_FREE
{
ge.getView().setAbstractView(currView);
var camera = ge.getView().copyAsCamera(ge.ALTITUDE_ABSOLUTE);
camera.setTilt(currFollowTilt);
camera.setRoll(0);
camera.setHeading(currFollowHeading);
ge.getView().setAbstractView(camera);
}
else
{
ge.getView().setAbstractView(currView);
currView.setTilt(currFollowTilt);
currView.setHeading(currFollowHeading);
}
ge.getView().setAbstractView(currView);
}
function failureCallback(object)

4
qgroundcontrol.pri

@ -42,8 +42,8 @@ macx { @@ -42,8 +42,8 @@ macx {
# COMPILER_VERSION = $$system(gcc -v)
#message(Using compiler $$COMPILER_VERSION)
CONFIG += x86_64 cocoa phonon
CONFIG -= x86
CONFIG += x86 cocoa phonon
CONFIG -= x86_64
#HARDWARE_PLATFORM = $$system(uname -a)
#contains( $$HARDWARE_PLATFORM, "9.6.0" ) || contains( $$HARDWARE_PLATFORM, "9.7.0" ) || contains( $$HARDWARE_PLATFORM, "9.8.0" ) || contains( $$HARDWARE_PLATFORM, "9.9.0" ) {

30
qgroundcontrol.pro

@ -542,19 +542,19 @@ TRANSLATIONS += es-MX.ts \ @@ -542,19 +542,19 @@ TRANSLATIONS += es-MX.ts \
# xbee support
# libxbee only supported by linux and windows systems
#win32-msvc2008|win32-msvc2010|linux{
# HEADERS += src/comm/XbeeLinkInterface.h \
# src/comm/XbeeLink.h \
# src/comm/HexSpinBox.h \
# src/ui/XbeeConfigurationWindow.h \
# src/comm/CallConv.h
# SOURCES += src/comm/XbeeLink.cpp \
# src/comm/HexSpinBox.cpp \
# src/ui/XbeeConfigurationWindow.cpp
# DEFINES += XBEELINK
# INCLUDEPATH += thirdParty/libxbee
win32-msvc2008|win32-msvc2010|linux{
HEADERS += src/comm/XbeeLinkInterface.h \
src/comm/XbeeLink.h \
src/comm/HexSpinBox.h \
src/ui/XbeeConfigurationWindow.h \
src/comm/CallConv.h
SOURCES += src/comm/XbeeLink.cpp \
src/comm/HexSpinBox.cpp \
src/ui/XbeeConfigurationWindow.cpp
DEFINES += XBEELINK
INCLUDEPATH += thirdParty/libxbee
# TO DO: build library when it does not exists already
# LIBS += -LthirdParty/libxbee/lib \
# -llibxbee
#
#}
LIBS += -LthirdParty/libxbee/lib \
-llibxbee
}

3
src/QGCCore.cc

@ -204,8 +204,9 @@ QGCCore::~QGCCore() @@ -204,8 +204,9 @@ QGCCore::~QGCCore()
{
//mainWindow->storeSettings();
mainWindow->close();
mainWindow->deleteLater();
//mainWindow->deleteLater();
// Delete singletons
delete MainWindow::instance();
delete LinkManager::instance();
delete UASManager::instance();
}

2
src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc

@ -96,7 +96,7 @@ void MAVLinkXMLParserV10::processError(QProcess::ProcessError err) @@ -96,7 +96,7 @@ void MAVLinkXMLParserV10::processError(QProcess::ProcessError err)
bool MAVLinkXMLParserV10::generate()
{
#ifdef Q_OS_WIN
QString generatorCall("%1/files/mavlink_generator/generator/mavgen.exe");
QString generatorCall("files/mavlink_generator/generator/mavgen.exe");
#endif
#if (defined Q_OS_MAC) || (defined Q_OS_LINUX)
QString generatorCall("python");

35
src/comm/HexSpinBox.cpp

@ -0,0 +1,35 @@ @@ -0,0 +1,35 @@
#include "HexSpinBox.h"
#include <qregexp.h>
HexSpinBox::HexSpinBox(QWidget *parent)
: QSpinBox(parent), validator(NULL)
{
setRange(0, 0x7fffffff);
validator = new QRegExpValidator(QRegExp("[0-9A-Fa-f]{1,8}"), this);
}
HexSpinBox::~HexSpinBox(void)
{
if(this->validator)
{
delete this->validator;
this->validator = NULL;
}
}
QValidator::State HexSpinBox::validate(QString &text, int &pos) const
{
return validator->validate(text, pos);
}
QString HexSpinBox::textFromValue(int value) const
{
return QString::number(value, 16).toUpper();
}
int HexSpinBox::valueFromText(const QString &text) const
{
bool ok;
return text.toInt(&ok, 16);
}

25
src/comm/HexSpinBox.h

@ -0,0 +1,25 @@ @@ -0,0 +1,25 @@
#ifndef HEXSPINBOX_H_
#define HEXSPINBOX_H_
#include <qspinbox.h>
class QRegExpValidator;
class HexSpinBox : public QSpinBox
{
Q_OBJECT
public:
HexSpinBox(QWidget *parent = 0);
~HexSpinBox(void);
protected:
QValidator::State validate(QString &text, int &pos) const;
int valueFromText(const QString &text) const;
QString textFromValue(int value) const;
private:
QRegExpValidator *validator;
};
#endif // HEXSPINBOX_H_

37
src/comm/MAVLinkSimulationLink.cc

@ -358,19 +358,19 @@ void MAVLinkSimulationLink::mainloop() @@ -358,19 +358,19 @@ void MAVLinkSimulationLink::mainloop()
if (rate10hzCounter == 1000 / rate / 9) {
rate10hzCounter = 1;
float lastX = x;
float lastY = y;
float lastZ = z;
float hackDt = 0.1f; // 100 ms
double lastX = x;
double lastY = y;
double lastZ = z;
double hackDt = 0.1f; // 100 ms
// Move X Position
x = 12.0*sin(((double)circleCounter)/200.0);
y = 5.0*cos(((double)circleCounter)/200.0);
z = 1.8 + 1.2*sin(((double)circleCounter)/200.0);
float xSpeed = (x - lastX)/hackDt;
float ySpeed = (y - lastY)/hackDt;
float zSpeed = (z - lastZ)/hackDt;
double xSpeed = (x - lastX)/hackDt;
double ySpeed = (y - lastY)/hackDt;
double zSpeed = (z - lastZ)/hackDt;
@ -414,12 +414,12 @@ void MAVLinkSimulationLink::mainloop() @@ -414,12 +414,12 @@ void MAVLinkSimulationLink::mainloop()
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;
// // GLOBAL POSITION VEHICLE 2
// mavlink_msg_global_position_int_pack(54, componentId, &ret, (473780.28137103+(x+0.002))*1E3, (85489.9892510421+((y/2)+0.3))*1E3, (z+570.0)*1000.0, xSpeed, ySpeed, zSpeed);
// bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
// //add data into datastream
// memcpy(stream+streampointer,buffer, bufferlength);
// streampointer += bufferlength;
// GLOBAL POSITION VEHICLE 2
mavlink_msg_global_position_int_pack(systemId+1, componentId+1, &ret, 0, (473780.28137103+(x+0.00001))*1E3, (85489.9892510421+((y/2)+0.00001))*1E3, (z+550.0)*1000.0, (z+550.0)*1000.0-1, xSpeed, ySpeed, zSpeed, yaw);
bufferlength = mavlink_msg_to_send_buffer(buffer, &ret);
//add data into datastream
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);
@ -427,7 +427,7 @@ void MAVLinkSimulationLink::mainloop() @@ -427,7 +427,7 @@ void MAVLinkSimulationLink::mainloop()
// // 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);
// mavlink_msg_global_position_int_pack(60, componentId, &ret, 0, (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);
// //add data into datastream
// memcpy(stream+streampointer,buffer, bufferlength);
@ -565,6 +565,15 @@ void MAVLinkSimulationLink::mainloop() @@ -565,6 +565,15 @@ void MAVLinkSimulationLink::mainloop()
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;
// Pack message and get size of encoded byte string
messageSize = mavlink_msg_heartbeat_pack(systemId+1, componentId+1, &msg, mavType, MAV_AUTOPILOT_GENERIC, system.base_mode, system.custom_mode, system.system_status);
// Allocate buffer with packet data
bufferlength = mavlink_msg_to_send_buffer(buffer, &msg);
//qDebug() << "CRC:" << msg.ck_a << msg.ck_b;
//add data into datastream
memcpy(stream+streampointer,buffer, bufferlength);
streampointer += bufferlength;
// Send controller states

4
src/comm/MAVLinkSimulationMAV.cc

@ -14,10 +14,10 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy @@ -14,10 +14,10 @@ MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int sy
timer1Hz(0),
latitude(lat),
longitude(lon),
altitude(0.0),
altitude(550.0),
x(lat),
y(lon),
z(550),
z(altitude),
roll(0.0),
pitch(0.0),
yaw(0.0),

2
src/comm/QGCFlightGearLink.cc

@ -188,7 +188,7 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size) @@ -188,7 +188,7 @@ void QGCFlightGearLink::writeBytes(const char* data, qint64 size)
void QGCFlightGearLink::readBytes()
{
const qint64 maxLength = 65536;
static char data[maxLength];
char data[maxLength];
QHostAddress sender;
quint16 senderPort;

2
src/comm/UDPLink.cc

@ -210,7 +210,7 @@ void UDPLink::writeBytes(const char* data, qint64 size) @@ -210,7 +210,7 @@ void UDPLink::writeBytes(const char* data, qint64 size)
void UDPLink::readBytes()
{
const qint64 maxLength = 65536;
static char data[maxLength];
char data[maxLength];
QHostAddress sender;
quint16 senderPort;

2
src/libs/opmapcontrol/src/core/providerstrings.cpp

@ -63,7 +63,7 @@ namespace core { @@ -63,7 +63,7 @@ namespace core {
/// Google Maps API generated using http://greatmaps.codeplex.com/
/// from http://code.google.com/intl/en-us/apis/maps/signup.html
/// </summary>
GoogleMapsAPIKey = "ABQIAAAAWaQgWiEBF3lW97ifKnAczhRAzBk5Igf8Z5n2W3hNnMT0j2TikxTLtVIGU7hCLLHMAuAMt-BO5UrEWA";
GoogleMapsAPIKey = "ABQIAAAA5Q6wxQ6lxKS8haLVdUJaqhSjosg_0jiTTs2iXtkDVG0n0If1mBRHzhWw5VqBZX-j4NuzoVpU-UaHVg";
// Yahoo version strings
VersionYahooMap = "4.3";

32
src/libs/opmapcontrol/src/core/pureimagecache.cpp

@ -227,23 +227,25 @@ namespace core { @@ -227,23 +227,25 @@ namespace core {
#endif //DEBUG_PUREIMAGECACHE
QString db=dir+"Data.qmdb";
QSqlDatabase cn;
cn = QSqlDatabase::addDatabase("QSQLITE",QString::number(id));
{
QSqlDatabase cn;
cn = QSqlDatabase::addDatabase("QSQLITE",QString::number(id));
cn.setDatabaseName(db);
cn.setConnectOptions("QSQLITE_ENABLE_SHARED_CACHE");
if(cn.open())
{
QSqlQuery query(cn);
query.exec(QString("SELECT Tile FROM TilesData WHERE id = (SELECT id FROM Tiles WHERE X=%1 AND Y=%2 AND Zoom=%3 AND Type=%4)").arg(pos.X()).arg(pos.Y()).arg(zoom).arg((int) type));
query.next();
if(query.isValid())
cn.setDatabaseName(db);
cn.setConnectOptions("QSQLITE_ENABLE_SHARED_CACHE");
if(cn.open())
{
ar=query.value(0).toByteArray();
}
cn.close();
}
QSqlQuery query(cn);
query.exec(QString("SELECT Tile FROM TilesData WHERE id = (SELECT id FROM Tiles WHERE X=%1 AND Y=%2 AND Zoom=%3 AND Type=%4)").arg(pos.X()).arg(pos.Y()).arg(zoom).arg((int) type));
query.next();
if(query.isValid())
{
ar=query.value(0).toByteArray();
}
cn.close();
}
}
QSqlDatabase::removeDatabase(QString::number(id));
lock.unlock();
return ar;

1
src/libs/opmapcontrol/src/mapwidget/waypointlineitem.cpp

@ -27,7 +27,6 @@ WaypointLineItem::WaypointLineItem(WayPointItem* wp1, WayPointItem* wp2, QColor @@ -27,7 +27,6 @@ WaypointLineItem::WaypointLineItem(WayPointItem* wp1, WayPointItem* wp2, QColor
setLine(localPoint1.X(), localPoint1.Y(), localPoint2.X(), localPoint2.Y());
// Connect updates
// Update line from both waypoints
connect(wp1, SIGNAL(WPValuesChanged(WayPointItem*)), this, SLOT(updateWPValues(WayPointItem*)));
connect(wp2, SIGNAL(WPValuesChanged(WayPointItem*)), this, SLOT(updateWPValues(WayPointItem*)));

15
src/uas/PxQuadMAV.cc

@ -42,9 +42,12 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -42,9 +42,12 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
#ifdef MAVLINK_ENABLED_PIXHAWK
mavlink_message_t* msg = &message;
if (message.sysid == uasId) {
switch (message.msgid) {
case MAVLINK_MSG_ID_RAW_AUX: {
if (message.sysid == uasId)
{
switch (message.msgid)
{
case MAVLINK_MSG_ID_RAW_AUX:
{
mavlink_raw_aux_t raw;
mavlink_msg_raw_aux_decode(&message, &raw);
quint64 time = getUnixTime(0);
@ -52,14 +55,16 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -52,14 +55,16 @@ void PxQuadMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit valueChanged(uasId, "Temperature", "raw", raw.temp, time);
}
break;
case MAVLINK_MSG_ID_IMAGE_TRIGGERED: {
case MAVLINK_MSG_ID_IMAGE_TRIGGERED:
{
// FIXME Kind of a hack to load data from disk
mavlink_image_triggered_t img;
mavlink_msg_image_triggered_decode(&message, &img);
emit imageStarted(img.timestamp);
}
break;
case MAVLINK_MSG_ID_PATTERN_DETECTED: {
case MAVLINK_MSG_ID_PATTERN_DETECTED:
{
mavlink_pattern_detected_t detected;
mavlink_msg_pattern_detected_decode(&message, &detected);
QByteArray b;

57
src/uas/UAS.cc

@ -82,6 +82,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(), @@ -82,6 +82,12 @@ UAS::UAS(MAVLinkProtocol* protocol, int id) : UASInterface(),
isGlobalPositionKnown(false),
systemIsArmed(false)
{
for (unsigned int i = 0; i<255;++i)
{
componentID[i] = -1;
componentMulti[i] = false;
}
color = UASInterface::getNextColor();
setBatterySpecs(QString("9V,9.5V,12.6V"));
connect(statusTimeout, SIGNAL(timeout()), this, SLOT(updateState()));
@ -210,6 +216,27 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -210,6 +216,27 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
QString uasState;
QString stateDescription;
bool multiComponentSourceDetected = false;
bool wrongComponent = false;
// Store component ID
if (componentID[message.msgid] == -1)
{
componentID[message.msgid] = message.compid;
}
else
{
// Got this message already
if (componentID[message.msgid] != message.compid)
{
componentMulti[message.msgid] = true;
wrongComponent = true;
}
}
if (componentMulti[message.msgid] == true) multiComponentSourceDetected = true;
switch (message.msgid)
{
case MAVLINK_MSG_ID_HEARTBEAT:
@ -232,6 +259,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -232,6 +259,9 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
case MAV_TYPE_QUADROTOR:
setAirframe(UASInterface::QGC_AIRFRAME_CHEETAH);
break;
case MAV_TYPE_HEXAROTOR:
setAirframe(UASInterface::QGC_AIRFRAME_HEXCOPTER);
break;
default:
// Do nothing
break;
@ -333,13 +363,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -333,13 +363,15 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
break;
case MAVLINK_MSG_ID_SYS_STATUS:
{
if (multiComponentSourceDetected && message.compid != MAV_COMP_ID_IMU_2)
{
break;
}
mavlink_sys_status_t state;
mavlink_msg_sys_status_decode(&message, &state);
emit loadChanged(this,state.load/10.0f);
currentVoltage = state.voltage_battery/1000.0f;
lpVoltage = filterVoltage(currentVoltage);
@ -370,6 +402,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -370,6 +402,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
break;
case MAVLINK_MSG_ID_ATTITUDE:
{
if (wrongComponent) break;
mavlink_attitude_t attitude;
mavlink_msg_attitude_decode(&message, &attitude);
quint64 time = getUnixReferenceTime(attitude.time_boot_ms);
@ -463,7 +497,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -463,7 +497,8 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
emit globalPositionChanged(this, latitude, longitude, altitude, time);
emit speedChanged(this, speedX, speedY, speedZ, time);
// Set internal state
if (!positionLock) {
if (!positionLock)
{
// If position was not locked before, notify positive
GAudioOutput::instance()->notifyPositive();
}
@ -483,29 +518,35 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message) @@ -483,29 +518,35 @@ void UAS::receiveMessage(LinkInterface* link, mavlink_message_t message)
// quint64 time = getUnixTime(pos.time_usec);
quint64 time = getUnixTime(pos.time_usec);
if (pos.fix_type > 2) {
if (pos.fix_type > 2)
{
emit globalPositionChanged(this, pos.lat/(double)1E7, pos.lon/(double)1E7, pos.alt/1000.0, time);
latitude = pos.lat/(double)1E7;
longitude = pos.lon/(double)1E7;
altitude = pos.alt/1000.0;
positionLock = true;
isGlobalPositionKnown = true;
// Check for NaN
int alt = pos.alt;
if (alt != alt) {
if (!isnan(alt) && !isinf(alt))
{
alt = 0;
emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN FOR ALTITUDE");
//emit textMessageReceived(uasId, message.compid, 255, "GCS ERROR: RECEIVED NaN or Inf FOR ALTITUDE");
}
// FIXME REMOVE LATER emit valueChanged(uasId, "altitude", "m", pos.alt/(double)1E3, time);
// Smaller than threshold and not NaN
float vel = pos.vel/100.0f;
if (vel < 1000000 && !isnan(vel) && !isinf(vel)) {
if (vel < 1000000 && !isnan(vel) && !isinf(vel))
{
// FIXME REMOVE LATER emit valueChanged(uasId, "speed", "m/s", vel, time);
//qDebug() << "GOT GPS RAW";
// emit speedChanged(this, (double)pos.v, 0.0, 0.0, time);
} else {
}
else
{
emit textMessageReceived(uasId, message.compid, 255, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
}
}

2
src/uas/UAS.h

@ -544,6 +544,8 @@ protected: @@ -544,6 +544,8 @@ protected:
quint64 getUnixTimeFromMs(quint64 time);
/** @brief Get the UNIX timestamp in milliseconds, ignore attitudeStamped mode */
quint64 getUnixReferenceTime(quint64 time);
int componentID[256];
bool componentMulti[256];
protected slots:
/** @brief Write settings to disk */

25
src/ui/HDDisplay.cc

@ -50,8 +50,8 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : @@ -50,8 +50,8 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) :
acceptUnitList(new QStringList()),
lastPaintTime(0),
columns(3),
valuesChanged(true)/*,
m_ui(new Ui::HDDisplay)*/
valuesChanged(true),
m_ui(NULL)
{
setWindowTitle(title);
//m_ui->setupUi(this);
@ -135,7 +135,22 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) : @@ -135,7 +135,22 @@ HDDisplay::HDDisplay(QStringList* plotList, QString title, QWidget *parent) :
HDDisplay::~HDDisplay()
{
saveState();
delete m_ui;
if(this->refreshTimer)
{
delete this->refreshTimer;
}
if(this->acceptList)
{
delete this->acceptList;
}
if(this->acceptUnitList)
{
delete this->acceptUnitList;
}
if(this->m_ui)
{
delete m_ui;
}
}
QSize HDDisplay::sizeHint() const
@ -186,9 +201,9 @@ void HDDisplay::triggerUpdate() @@ -186,9 +201,9 @@ void HDDisplay::triggerUpdate()
void HDDisplay::paintEvent(QPaintEvent * event)
{
Q_UNUSED(event);
static quint64 interval = 0;
quint64 interval = 0;
//qDebug() << "INTERVAL:" << MG::TIME::getGroundTimeNow() - interval << __FILE__ << __LINE__;
interval = MG::TIME::getGroundTimeNow();
interval = QGC::groundTimeMilliseconds();
renderOverlay();
}

5
src/ui/HSIDisplay.cc

@ -136,6 +136,11 @@ HSIDisplay::HSIDisplay(QWidget *parent) : @@ -136,6 +136,11 @@ HSIDisplay::HSIDisplay(QWidget *parent) :
setFocusPolicy(Qt::StrongFocus);
}
HSIDisplay::~HSIDisplay()
{
}
void HSIDisplay::resetMAVState()
{
mavInitialized = false;

2
src/ui/HSIDisplay.h

@ -48,7 +48,7 @@ class HSIDisplay : public HDDisplay @@ -48,7 +48,7 @@ class HSIDisplay : public HDDisplay
Q_OBJECT
public:
HSIDisplay(QWidget *parent = 0);
// ~HSIDisplay();
~HSIDisplay();
public slots:
void setActiveUAS(UASInterface* uas);

27
src/ui/MAVLinkDecoder.cc

@ -7,6 +7,11 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) : @@ -7,6 +7,11 @@ MAVLinkDecoder::MAVLinkDecoder(MAVLinkProtocol* protocol, QObject *parent) :
mavlink_message_info_t msg[256] = MAVLINK_MESSAGE_INFO;
memcpy(messageInfo, msg, sizeof(mavlink_message_info_t)*256);
memset(receivedMessages, 0, sizeof(mavlink_message_t)*256);
for (unsigned int i = 0; i<255;++i)
{
componentID[i] = -1;
componentMulti[i] = false;
}
// Fill filter
messageFilter.insert(MAVLINK_MSG_ID_HEARTBEAT, false);
@ -69,6 +74,26 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag @@ -69,6 +74,26 @@ void MAVLinkDecoder::receiveMessage(LinkInterface* link,mavlink_message_t messag
void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 time)
{
bool multiComponentSourceDetected = false;
bool wrongComponent = false;
// Store component ID
if (componentID[msg->msgid] == -1)
{
componentID[msg->msgid] = msg->compid;
}
else
{
// Got this message already
if (componentID[msg->msgid] != msg->compid)
{
componentMulti[msg->msgid] = true;
wrongComponent = true;
}
}
if (componentMulti[msg->msgid] == true) multiComponentSourceDetected = true;
// Add field tree widget item
uint8_t msgid = msg->msgid;
if (messageFilter.contains(msgid)) return;
@ -78,6 +103,8 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64 @@ -78,6 +103,8 @@ void MAVLinkDecoder::emitFieldValue(mavlink_message_t* msg, int fieldid, quint64
QString name("%1.%2");
QString unit("");
name = name.arg(messageInfo[msgid].name, fieldName);
if (multiComponentSourceDetected) name.prepend(QString("C%1:").arg(msg->compid));
name.prepend(QString("M%1:").arg(msg->sysid));
switch (messageInfo[msgid].fields[fieldid].type)
{
case MAVLINK_TYPE_CHAR:

2
src/ui/MAVLinkDecoder.h

@ -30,6 +30,8 @@ protected: @@ -30,6 +30,8 @@ protected:
mavlink_message_info_t messageInfo[256]; ///< Message information
QMap<uint16_t, bool> messageFilter; ///< Message/field names not to emit
QMap<uint16_t, bool> textMessageFilter; ///< Message/field names not to emit in text mode
int componentID[256]; ///< Multi component detection
bool componentMulti[256]; ///< Multi components detected
};

121
src/ui/MainWindow.cc

@ -91,7 +91,7 @@ MainWindow::MainWindow(QWidget *parent): @@ -91,7 +91,7 @@ MainWindow::MainWindow(QWidget *parent):
currentStyle(QGC_MAINWINDOW_STYLE_INDOOR),
aboutToCloseFlag(false),
changingViewsFlag(false),
centerStackActionGroup(this),
centerStackActionGroup(new QActionGroup(this)),
styleFileName(QCoreApplication::applicationDirPath() + "/style-indoor.css"),
autoReconnect(false),
lowPowerMode(false)
@ -138,7 +138,7 @@ MainWindow::MainWindow(QWidget *parent): @@ -138,7 +138,7 @@ MainWindow::MainWindow(QWidget *parent):
setCorner(Qt::BottomRightCorner, Qt::RightDockWidgetArea);
// Setup UI state machines
centerStackActionGroup.setExclusive(true);
centerStackActionGroup->setExclusive(true);
centerStack = new QStackedWidget(this);
setCentralWidget(centerStack);
@ -246,8 +246,8 @@ MainWindow::~MainWindow() @@ -246,8 +246,8 @@ MainWindow::~MainWindow()
if (dockWidget)
{
// Remove dock widget from main window
removeDockWidget(dockWidget);
delete dockWidget->widget();
// removeDockWidget(dockWidget);
// delete dockWidget->widget();
delete dockWidget;
}
else
@ -417,14 +417,14 @@ void MainWindow::buildCommonWidgets() @@ -417,14 +417,14 @@ void MainWindow::buildCommonWidgets()
parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET");
addTool(parametersDockWidget, tr("Calibration and Parameters"), Qt::RightDockWidgetArea);
}
if (!hsiDockWidget) {
hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this);
hsiDockWidget->setWidget( new HSIDisplay(this) );
hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET");
addTool(hsiDockWidget, tr("Horizontal Situation"), Qt::BottomDockWidgetArea);
}
if (!headDown1DockWidget) {
headDown1DockWidget = new QDockWidget(tr("Flight Display"), this);
HDDisplay* hdDisplay = new HDDisplay(acceptList, "Flight Display", this);
@ -442,7 +442,7 @@ void MainWindow::buildCommonWidgets() @@ -442,7 +442,7 @@ void MainWindow::buildCommonWidgets()
headDown2DockWidget->setObjectName("HEAD_DOWN_DISPLAY_2_DOCK_WIDGET");
addTool(headDown2DockWidget, tr("Actuator Status"), Qt::RightDockWidgetArea);
}
if (!rcViewDockWidget) {
rcViewDockWidget = new QDockWidget(tr("Radio Control"), this);
rcViewDockWidget->setWidget( new QGCRemoteControlView(this) );
@ -555,7 +555,7 @@ void MainWindow::addCentralWidget(QWidget* widget, const QString& title) @@ -555,7 +555,7 @@ void MainWindow::addCentralWidget(QWidget* widget, const QString& title)
QVariant var;
var.setValue((QWidget*)widget);
tempAction->setData(var);
centerStackActionGroup.addAction(tempAction);
centerStackActionGroup->addAction(tempAction);
connect(tempAction,SIGNAL(triggered()),this, SLOT(showCentralWidget()));
connect(widget, SIGNAL(visibilityChanged(bool)), tempAction, SLOT(setChecked(bool)));
tempAction->setChecked(widget->isVisible());
@ -1503,108 +1503,3 @@ QList<QAction*> MainWindow::listLinkMenuActions(void) @@ -1503,108 +1503,3 @@ QList<QAction*> MainWindow::listLinkMenuActions(void)
{
return ui.menuNetwork->actions();
}
/*
void MainWindow::buildSenseSoarWidgets()
{
if (!linechartWidget)
{
// Center widgets
linechartWidget = new Linecharts(this);
addToCentralWidgetsMenu(linechartWidget, tr("Realtime Plot"), SLOT(showCentralWidget()), CENTRAL_LINECHART);
}
if (!hudWidget)
{
hudWidget = new HUD(320, 240, this);
addToCentralWidgetsMenu(hudWidget, tr("Head Up Display"), SLOT(showCentralWidget()), CENTRAL_HUD);
}
if (!dataplotWidget) {
dataplotWidget = new QGCDataPlot2D(this);
addToCentralWidgetsMenu(dataplotWidget, "Logfile Plot", SLOT(showCentralWidget()), CENTRAL_DATA_PLOT);
}
#ifdef QGC_OSG_ENABLED
if (!_3DWidget) {
_3DWidget = Q3DWidgetFactory::get("PIXHAWK");
addToCentralWidgetsMenu(_3DWidget, tr("Local 3D"), SLOT(showCentralWidget()), CENTRAL_3D_LOCAL);
}
#endif
#ifdef QGC_OSGEARTH_ENABLED
if (!_3DMapWidget) {
_3DMapWidget = Q3DWidgetFactory::get("MAP3D");
addToCentralWidgetsMenu(_3DMapWidget, tr("OSG Earth 3D"), SLOT(showCentralWidget()), CENTRAL_OSGEARTH);
}
#endif
#if (defined _MSC_VER) | (defined Q_OS_MAC)
if (!gEarthWidget) {
gEarthWidget = new QGCGoogleEarthView(this);
addToCentralWidgetsMenu(gEarthWidget, tr("Google Earth"), SLOT(showCentralWidget()), CENTRAL_GOOGLE_EARTH);
}
#endif
// Dock widgets
if (!parametersDockWidget) {
parametersDockWidget = new QDockWidget(tr("Calibration and Onboard Parameters"), this);
parametersDockWidget->setWidget( new ParameterInterface(this) );
parametersDockWidget->setObjectName("PARAMETER_INTERFACE_DOCKWIDGET");
addToToolsMenu (parametersDockWidget, tr("Calibration and Parameters"), SLOT(showToolWidget(bool)), MENU_PARAMETERS, Qt::RightDockWidgetArea);
}
if (!hsiDockWidget) {
hsiDockWidget = new QDockWidget(tr("Horizontal Situation Indicator"), this);
hsiDockWidget->setWidget( new HSIDisplay(this) );
hsiDockWidget->setObjectName("HORIZONTAL_SITUATION_INDICATOR_DOCK_WIDGET");
addToToolsMenu (hsiDockWidget, tr("Horizontal Situation"), SLOT(showToolWidget(bool)), MENU_HSI, Qt::BottomDockWidgetArea);
}
if (!rcViewDockWidget) {
rcViewDockWidget = new QDockWidget(tr("Radio Control"), this);
rcViewDockWidget->setWidget( new QGCRemoteControlView(this) );
rcViewDockWidget->setObjectName("RADIO_CONTROL_CHANNELS_DOCK_WIDGET");
addToToolsMenu (rcViewDockWidget, tr("Radio Control"), SLOT(showToolWidget(bool)), MENU_RC_VIEW, Qt::BottomDockWidgetArea);
}
if (!headUpDockWidget) {
headUpDockWidget = new QDockWidget(tr("HUD"), this);
headUpDockWidget->setWidget( new HUD(320, 240, this));
headUpDockWidget->setObjectName("HEAD_UP_DISPLAY_DOCK_WIDGET");
addToToolsMenu (headUpDockWidget, tr("Head Up Display"), SLOT(showToolWidget(bool)), MENU_HUD, Qt::RightDockWidgetArea);
}
}
void MainWindow::connectSenseSoarWidgets()
{
}
void MainWindow::arrangeSenseSoarCenterStack()
{
if (!centerStack) {
qDebug() << "Center Stack not Created!";
return;
}
if (linechartWidget && (centerStack->indexOf(linechartWidget) == -1)) centerStack->addWidget(linechartWidget);
#ifdef QGC_OSG_ENABLED
if (_3DWidget && (centerStack->indexOf(_3DWidget) == -1)) centerStack->addWidget(_3DWidget);
#endif
#ifdef QGC_OSGEARTH_ENABLED
if (_3DMapWidget && (centerStack->indexOf(_3DMapWidget) == -1)) centerStack->addWidget(_3DMapWidget);
#endif
#if (defined _MSC_VER) | (defined Q_OS_MAC)
if (gEarthWidget && (centerStack->indexOf(gEarthWidget) == -1)) centerStack->addWidget(gEarthWidget);
#endif
if (hudWidget && (centerStack->indexOf(hudWidget) == -1)) centerStack->addWidget(hudWidget);
if (dataplotWidget && (centerStack->indexOf(dataplotWidget) == -1)) centerStack->addWidget(dataplotWidget);
}
void MainWindow::connectSenseSoarActions()
{
}
*/

2
src/ui/MainWindow.h

@ -303,7 +303,7 @@ protected: @@ -303,7 +303,7 @@ protected:
QSettings settings;
QStackedWidget *centerStack;
QActionGroup centerStackActionGroup;
QActionGroup* centerStackActionGroup;
// Center widgets
QPointer<Linecharts> linechartWidget;

9
src/ui/QGCMAVLinkInspector.cc

@ -41,7 +41,6 @@ void QGCMAVLinkInspector::refreshView() @@ -41,7 +41,6 @@ void QGCMAVLinkInspector::refreshView()
messageName = messageName.arg(messageInfo[msg->msgid].name).arg(messagesHz.value(msg->msgid, 0), 2, 'f', 0).arg(msg->msgid);
if (!treeWidgetItems.contains(msg->msgid))
{
QStringList fields;
fields << messageName;
QTreeWidgetItem* widget = new QTreeWidgetItem(fields);
@ -72,8 +71,6 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m @@ -72,8 +71,6 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
{
Q_UNUSED(link);
// Only overwrite if system filter is set
// int filterValue = ui->systemComboBox()->value().toInt();
// if (filterValue != )
memcpy(receivedMessages+message.msgid, &message, sizeof(mavlink_message_t));
float msgHz = 0.0f;
@ -85,14 +82,10 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m @@ -85,14 +82,10 @@ void QGCMAVLinkInspector::receiveMessage(LinkInterface* link,mavlink_message_t m
{
msgHz = 1;
}
//qDebug() << "DIFF:" << receiveTime - lastMessageUpdate.value(message.msgid);
float newHz = 0.001f*msgHz+0.999f*messagesHz.value(message.msgid, 1);
qDebug() << "HZ" << newHz;
float newHz = 0.05f*msgHz+0.95f*messagesHz.value(message.msgid, 1);
messagesHz.insert(message.msgid, newHz);
}
//qDebug() << "MSGHZ:" << messagesHz.value(message.msgid, 1000);
lastMessageUpdate.insert(message.msgid, receiveTime);
}

16
src/ui/QGCRemoteControlView.cc

@ -43,8 +43,8 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) : @@ -43,8 +43,8 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) :
uasId(-1),
rssi(0.0f),
updated(false),
channelLayout(new QVBoxLayout())//,
//ui(new Ui::QGCRemoteControlView)
channelLayout(new QVBoxLayout()),
ui(NULL)
{
ui->setupUi(this);
QGridLayout* layout = new QGridLayout(this);
@ -71,8 +71,14 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) : @@ -71,8 +71,14 @@ QGCRemoteControlView::QGCRemoteControlView(QWidget *parent) :
QGCRemoteControlView::~QGCRemoteControlView()
{
delete ui;
delete channelLayout;
if(this->ui)
{
delete ui;
}
if(this->channelLayout)
{
delete channelLayout;
}
}
void QGCRemoteControlView::setUASId(int id)
@ -172,7 +178,7 @@ void QGCRemoteControlView::setRemoteRSSI(float rssiNormalized) @@ -172,7 +178,7 @@ void QGCRemoteControlView::setRemoteRSSI(float rssiNormalized)
void QGCRemoteControlView::appendChannelWidget(int channelId)
{
// Create new layout
QHBoxLayout* layout = new QHBoxLayout();
QHBoxLayout* layout = new QHBoxLayout(this);
// Add content
layout->addWidget(new QLabel(QString("Channel %1").arg(channelId + 1), this));
QLabel* raw = new QLabel(this);

29
src/ui/map3D/QGCGoogleEarthView.cc

@ -54,15 +54,15 @@ QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) : @@ -54,15 +54,15 @@ QGCGoogleEarthView::QGCGoogleEarthView(QWidget *parent) :
#ifdef _MSC_VER
// Create layout and attach webViewWin
QFile file("doc.html");
if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
qDebug() << __FILE__ << __LINE__ << "Could not open log file";
// QFile file("doc.html");
// if (!file.open(QIODevice::WriteOnly | QIODevice::Text))
// qDebug() << __FILE__ << __LINE__ << "Could not open log file";
QTextStream out(&file);
out << webViewWin->generateDocumentation();
out.flush();
file.flush();
file.close();
// QTextStream out(&file);
// out << webViewWin->generateDocumentation();
// out.flush();
// file.flush();
// file.close();
#else
@ -284,8 +284,6 @@ void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lat, dou @@ -284,8 +284,6 @@ void QGCGoogleEarthView::updateGlobalPosition(UASInterface* uas, double lat, dou
{
Q_UNUSED(usec);
javaScript(QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 22).arg(lon, 0, 'f', 22).arg(alt, 0, 'f', 22));
//qDebug() << QString("addTrailPosition(%1, %2, %3, %4);").arg(uas->getUASID()).arg(lat, 0, 'f', 15).arg(lon, 0, 'f', 15).arg(alt, 0, 'f', 15);
}
void QGCGoogleEarthView::clearTrails()
@ -614,7 +612,8 @@ void QGCGoogleEarthView::updateState() @@ -614,7 +612,8 @@ void QGCGoogleEarthView::updateState()
#if (QGC_EVENTLOOP_DEBUG)
qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif
if (gEarthInitialized) {
if (gEarthInitialized)
{
int uasId = 0;
double lat = 47.3769;
double lon = 8.549444;
@ -628,6 +627,8 @@ void QGCGoogleEarthView::updateState() @@ -628,6 +627,8 @@ void QGCGoogleEarthView::updateState()
QList<UASInterface*> mavs = UASManager::instance()->getUASList();
foreach (UASInterface* currMav, mavs)
{
// Only update if known
if (!currMav->globalPositionKnown()) continue;
uasId = currMav->getUASID();
lat = currMav->getLatitude();
lon = currMav->getLongitude();
@ -640,9 +641,9 @@ void QGCGoogleEarthView::updateState() @@ -640,9 +641,9 @@ void QGCGoogleEarthView::updateState()
javaScript(QString("setAircraftPositionAttitude(%1, %2, %3, %4, %6, %7, %8);")
.arg(uasId)
.arg(lat, 0, 'f', 15)
.arg(lon, 0, 'f', 15)
.arg(alt, 0, 'f', 15)
.arg(lat, 0, 'f', 22)
.arg(lon, 0, 'f', 22)
.arg(alt, 0, 'f', 22)
.arg(roll, 0, 'f', 9)
.arg(pitch, 0, 'f', 9)
.arg(yaw, 0, 'f', 9));

189
src/ui/uas/UASView.cc

@ -35,7 +35,6 @@ This file is part of the PIXHAWK project @@ -35,7 +35,6 @@ This file is part of the PIXHAWK project
#include <QInputDialog>
#include "QGC.h"
#include "MG.h"
#include "UASManager.h"
#include "UASView.h"
#include "UASWaypointManager.h"
@ -65,6 +64,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : @@ -65,6 +64,7 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
alt(0),
groundDistance(0),
localFrame(false),
globalFrameKnown(false),
removeAction(new QAction("Delete this system", this)),
renameAction(new QAction("Rename..", this)),
selectAction(new QAction("Control this system", this )),
@ -72,6 +72,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : @@ -72,6 +72,8 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
selectAirframeAction(new QAction("Choose Airframe", this)),
setBatterySpecsAction(new QAction("Set Battery Options", this)),
lowPowerModeEnabled(true),
generalUpdateCount(0),
filterTime(0),
m_ui(new Ui::UASView)
{
// FIXME XXX
@ -127,9 +129,12 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : @@ -127,9 +129,12 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
// Set static values
// Name
if (uas->getUASName() == "") {
if (uas->getUASName() == "")
{
m_ui->nameLabel->setText(tr("UAS") + QString::number(uas->getUASID()));
} else {
}
else
{
m_ui->nameLabel->setText(uas->getUASName());
}
@ -141,7 +146,9 @@ UASView::UASView(UASInterface* uas, QWidget *parent) : @@ -141,7 +146,9 @@ UASView::UASView(UASInterface* uas, QWidget *parent) :
if (lowPowerModeEnabled)
{
refreshTimer->start(updateInterval*3);
} else {
}
else
{
refreshTimer->start(updateInterval);
}
@ -194,10 +201,13 @@ void UASView::setBackgroundColor() @@ -194,10 +201,13 @@ void UASView::setBackgroundColor()
QColor uasColor = uas->getColor();
QString colorstyle;
QString borderColor = "#4A4A4F";
if (isActive) {
if (isActive)
{
borderColor = "#FA4A4F";
uasColor = uasColor.darker(475);
} else {
}
else
{
uasColor = uasColor.darker(675);
}
colorstyle = colorstyle.sprintf("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; background-color: #%02X%02X%02X; border: 2px solid %s; }",
@ -207,14 +217,16 @@ void UASView::setBackgroundColor() @@ -207,14 +217,16 @@ void UASView::setBackgroundColor()
void UASView::setUASasActive(bool active)
{
if (active) {
if (active)
{
UASManager::instance()->setActiveUAS(this->uas);
}
}
void UASView::updateActiveUAS(UASInterface* uas, bool active)
{
if (uas == this->uas) {
if (uas == this->uas)
{
this->isActive = active;
setBackgroundColor();
}
@ -239,22 +251,25 @@ void UASView::mouseDoubleClickEvent (QMouseEvent * event) @@ -239,22 +251,25 @@ void UASView::mouseDoubleClickEvent (QMouseEvent * event)
void UASView::enterEvent(QEvent* event)
{
if (event->type() == QEvent::MouseMove) {
if (event->type() == QEvent::MouseMove)
{
emit uasInFocus(uas);
if (uas != UASManager::instance()->getActiveUAS()) {
if (uas != UASManager::instance()->getActiveUAS())
{
grabMouse(QCursor(Qt::PointingHandCursor));
}
}
// qDebug() << __FILE__ << __LINE__ << "IN FOCUS";
if (event->type() == QEvent::MouseButtonDblClick) {
if (event->type() == QEvent::MouseButtonDblClick)
{
// qDebug() << __FILE__ << __LINE__ << "UAS CLICKED!";
}
}
void UASView::leaveEvent(QEvent* event)
{
if (event->type() == QEvent::MouseMove) {
if (event->type() == QEvent::MouseMove)
{
emit uasOutFocus(uas);
releaseMouse();
}
@ -300,9 +315,11 @@ void UASView::updateName(const QString& name) @@ -300,9 +315,11 @@ void UASView::updateName(const QString& name)
*/
void UASView::setSystemType(UASInterface* uas, unsigned int systemType)
{
if (uas == this->uas) {
if (uas == this->uas)
{
// Set matching icon
switch (systemType) {
switch (systemType)
{
case 0:
m_ui->typeButton->setIcon(QIcon(":/images/mavs/generic.svg"));
break;
@ -354,9 +371,7 @@ void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double @@ -354,9 +371,7 @@ void UASView::updateLocalPosition(UASInterface* uas, double x, double y, double
this->x = x;
this->y = y;
this->z = z;
if (!localFrame) {
localFrame = true;
}
localFrame = true;
}
void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, double alt, quint64 usec)
@ -366,6 +381,7 @@ void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, do @@ -366,6 +381,7 @@ void UASView::updateGlobalPosition(UASInterface* uas, double lon, double lat, do
this->lon = lon;
this->lat = lat;
this->alt = alt;
globalFrameKnown = true;
}
void UASView::updateSpeed(UASInterface*, double x, double y, double z, quint64 usec)
@ -386,8 +402,10 @@ void UASView::setWaypoint(int uasId, int id, double x, double y, double z, doubl @@ -386,8 +402,10 @@ void UASView::setWaypoint(int uasId, int id, double x, double y, double z, doubl
Q_UNUSED(z);
Q_UNUSED(yaw);
Q_UNUSED(autocontinue);
if (uasId == this->uas->getUASID()) {
if (current) {
if (uasId == this->uas->getUASID())
{
if (current)
{
m_ui->waypointLabel->setText(tr("WP") + QString::number(id));
}
}
@ -395,14 +413,16 @@ void UASView::setWaypoint(int uasId, int id, double x, double y, double z, doubl @@ -395,14 +413,16 @@ void UASView::setWaypoint(int uasId, int id, double x, double y, double z, doubl
void UASView::selectWaypoint(int uasId, int id)
{
if (uasId == this->uas->getUASID()) {
if (uasId == this->uas->getUASID())
{
m_ui->waypointLabel->setText(tr("WP") + QString::number(id));
}
}
void UASView::updateThrust(UASInterface* uas, double thrust)
{
if (this->uas == uas) {
if (this->uas == uas)
{
this->thrust = thrust;
}
}
@ -410,7 +430,8 @@ void UASView::updateThrust(UASInterface* uas, double thrust) @@ -410,7 +430,8 @@ void UASView::updateThrust(UASInterface* uas, double thrust)
void UASView::updateBattery(UASInterface* uas, double voltage, double percent, int seconds)
{
Q_UNUSED(voltage);
if (this->uas == uas) {
if (this->uas == uas)
{
timeRemaining = seconds;
chargeLevel = percent;
}
@ -418,7 +439,8 @@ void UASView::updateBattery(UASInterface* uas, double voltage, double percent, i @@ -418,7 +439,8 @@ void UASView::updateBattery(UASInterface* uas, double voltage, double percent, i
void UASView::updateState(UASInterface* uas, QString uasState, QString stateDescription)
{
if (this->uas == uas) {
if (this->uas == uas)
{
state = uasState;
stateDesc = stateDescription;
}
@ -426,7 +448,8 @@ void UASView::updateState(UASInterface* uas, QString uasState, QString stateDesc @@ -426,7 +448,8 @@ void UASView::updateState(UASInterface* uas, QString uasState, QString stateDesc
void UASView::updateLoad(UASInterface* uas, double load)
{
if (this->uas == uas) {
if (this->uas == uas)
{
this->load = load;
}
}
@ -437,7 +460,8 @@ void UASView::contextMenuEvent (QContextMenuEvent* event) @@ -437,7 +460,8 @@ void UASView::contextMenuEvent (QContextMenuEvent* event)
menu.addAction(selectAction);
menu.addSeparator();
menu.addAction(renameAction);
if (timeout) {
if (timeout)
{
menu.addAction(removeAction);
}
menu.addAction(hilAction);
@ -448,7 +472,8 @@ void UASView::contextMenuEvent (QContextMenuEvent* event) @@ -448,7 +472,8 @@ void UASView::contextMenuEvent (QContextMenuEvent* event)
void UASView::setBatterySpecs()
{
if (uas) {
if (uas)
{
bool ok;
QString newName = QInputDialog::getText(this, tr("Set Battery Specifications for %1").arg(uas->getUASName()),
tr("Specs: (empty,warn,full), e.g. (9V,9.5V,12.6V) or just warn level in percent (e.g. 15%) to use estimate from MAV"), QLineEdit::Normal,
@ -460,7 +485,8 @@ void UASView::setBatterySpecs() @@ -460,7 +485,8 @@ void UASView::setBatterySpecs()
void UASView::rename()
{
if (uas) {
if (uas)
{
bool ok;
QString newName = QInputDialog::getText(this, tr("Rename System %1").arg(uas->getUASName()),
tr("System Name:"), QLineEdit::Normal,
@ -472,7 +498,8 @@ void UASView::rename() @@ -472,7 +498,8 @@ void UASView::rename()
void UASView::selectAirframe()
{
if (uas) {
if (uas)
{
// Get list of airframes from UAS
QStringList airframes;
airframes << "Generic"
@ -484,12 +511,14 @@ void UASView::selectAirframe() @@ -484,12 +511,14 @@ void UASView::selectAirframe()
<< "Reaper"
<< "Predator"
<< "Coaxial"
<< "Pteryx";
<< "Pteryx"
<< "Asctec Firefly";
bool ok;
QString item = QInputDialog::getItem(this, tr("Select Airframe for %1").arg(uas->getUASName()),
tr("Airframe"), airframes, uas->getAirframe(), false, &ok);
if (ok && !item.isEmpty()) {
if (ok && !item.isEmpty())
{
// Set this airframe as UAS airframe
uas->setAirframe(airframes.indexOf(item));
}
@ -501,15 +530,15 @@ void UASView::refresh() @@ -501,15 +530,15 @@ void UASView::refresh()
//setUpdatesEnabled(false);
//setUpdatesEnabled(true);
//repaint();
//qDebug() << "UPDATING UAS WIDGET!" << uas->getUASName();
static quint64 lastupdate = 0;
//// qDebug() << "UASVIEW update diff: " << MG::TIME::getGroundTimeNow() - lastupdate;
lastupdate = MG::TIME::getGroundTimeNow();
// FIXME
static int generalUpdateCount = 0;
quint64 lastupdate = 0;
//// qDebug() << "UASVIEW update diff: " << MG::TIME::getGroundTimeNow() - lastupdate;
lastupdate = QGC::groundTimeMilliseconds();
if (generalUpdateCount == 4) {
if (generalUpdateCount == 4)
{
#if (QGC_EVENTLOOP_DEBUG)
// qDebug() << "EVENTLOOP:" << __FILE__ << __LINE__;
#endif
@ -525,29 +554,48 @@ void UASView::refresh() @@ -525,29 +554,48 @@ void UASView::refresh()
m_ui->thrustBar->setValue(this->thrust);
// Position
QString position;
position = position.sprintf("%05.1f %05.1f %06.1f m", x, y, z);
m_ui->positionLabel->setText(position);
QString globalPosition;
QString latIndicator;
if (lat > 0) {
latIndicator = "N";
} else {
latIndicator = "S";
// If global position is known, prefer it over local coordinates
if (!globalFrameKnown && localFrame)
{
QString position;
position = position.sprintf("%05.1f %05.1f %06.1f m", x, y, z);
m_ui->positionLabel->setText(position);
}
QString lonIndicator;
if (lon > 0) {
lonIndicator = "E";
} else {
lonIndicator = "W";
if (globalFrameKnown)
{
QString globalPosition;
QString latIndicator;
if (lat > 0)
{
latIndicator = "N";
}
else
{
latIndicator = "S";
}
QString lonIndicator;
if (lon > 0)
{
lonIndicator = "E";
}
else
{
lonIndicator = "W";
}
globalPosition = globalPosition.sprintf("%05.1f%s %05.1f%s %06.1f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
m_ui->positionLabel->setText(globalPosition);
}
globalPosition = globalPosition.sprintf("%05.1f%s %05.1f%s %06.1f m", lon, lonIndicator.toStdString().c_str(), lat, latIndicator.toStdString().c_str(), alt);
m_ui->positionLabel->setText(globalPosition);
// Altitude
if (groundDistance == 0 && alt != 0) {
if (groundDistance == 0 && alt != 0)
{
m_ui->groundDistanceLabel->setText(QString("%1 m").arg(alt, 6, 'f', 1, '0'));
} else {
}
else
{
m_ui->groundDistanceLabel->setText(QString("%1 m").arg(groundDistance, 6, 'f', 1, '0'));
}
@ -558,9 +606,10 @@ void UASView::refresh() @@ -558,9 +606,10 @@ void UASView::refresh()
// Thrust
m_ui->thrustBar->setValue(thrust * 100);
if(this->timeRemaining > 1 && this->timeRemaining < QGC::MAX_FLIGHT_TIME) {
if(this->timeRemaining > 1 && this->timeRemaining < QGC::MAX_FLIGHT_TIME)
{
// Filter output to get a higher stability
static double filterTime = static_cast<int>(this->timeRemaining);
filterTime = static_cast<int>(this->timeRemaining);
filterTime = 0.8 * filterTime + 0.2 * static_cast<int>(this->timeRemaining);
int sec = static_cast<int>(filterTime - static_cast<int>(filterTime / 60.0f) * 60);
int min = static_cast<int>(filterTime / 60);
@ -569,7 +618,9 @@ void UASView::refresh() @@ -569,7 +618,9 @@ void UASView::refresh()
QString timeText;
timeText = timeText.sprintf("%02d:%02d:%02d", hours, min, sec);
m_ui->timeRemainingLabel->setText(timeText);
} else {
}
else
{
m_ui->timeRemainingLabel->setText(tr("Calc.."));
}
@ -588,29 +639,37 @@ void UASView::refresh() @@ -588,29 +639,37 @@ void UASView::refresh()
QString colorstyle("QGroupBox { border-radius: 5px; padding: 2px; margin: 0px; border: 0px; background-color: %1; }");
if (timeout) {
if (timeout)
{
// CRITICAL CONDITION, NO HEARTBEAT
QString borderColor = "#FFFF00";
if (isActive) {
if (isActive)
{
borderColor = "#FA4A4F";
}
if (iconIsRed) {
if (iconIsRed)
{
QColor warnColor(Qt::red);
m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name()));
QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name());
m_ui->uasViewFrame->setStyleSheet(style);
} else {
}
else
{
QColor warnColor(Qt::black);
m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(warnColor.name()));
QString style = QString("QGroupBox { border-radius: 12px; padding: 0px; margin: 0px; border: 2px solid %1; background-color: %2; }").arg(borderColor, warnColor.name());
m_ui->uasViewFrame->setStyleSheet(style);
refreshTimer->setInterval(errorUpdateInterval);
refreshTimer->start();
}
iconIsRed = !iconIsRed;
} else {
}
else
{
if (!lowPowerModeEnabled)
{
// Fade heartbeat icon
@ -620,6 +679,7 @@ void UASView::refresh() @@ -620,6 +679,7 @@ void UASView::refresh()
//m_ui->heartbeatIcon->setAutoFillBackground(true);
m_ui->heartbeatIcon->setStyleSheet(colorstyle.arg(heartbeatColor.name()));
refreshTimer->setInterval(updateInterval);
refreshTimer->start();
}
}
//setUpdatesEnabled(true);
@ -630,7 +690,8 @@ void UASView::refresh() @@ -630,7 +690,8 @@ void UASView::refresh()
void UASView::changeEvent(QEvent *e)
{
QWidget::changeEvent(e);
switch (e->type()) {
switch (e->type())
{
case QEvent::LanguageChange:
m_ui->retranslateUi(this);
break;

3
src/ui/uas/UASView.h

@ -117,6 +117,7 @@ protected: @@ -117,6 +117,7 @@ protected:
float alt;
float groundDistance;
bool localFrame;
bool globalFrameKnown;
QAction* removeAction;
QAction* renameAction;
QAction* selectAction;
@ -126,6 +127,8 @@ protected: @@ -126,6 +127,8 @@ protected:
static const int updateInterval = 800;
static const int errorUpdateInterval = 200;
bool lowPowerModeEnabled; ///< Low power mode reduces update rates
unsigned int generalUpdateCount; ///< Skip counter for updates
double filterTime; ///< Filter time estimate of battery
void mouseDoubleClickEvent (QMouseEvent * event);

Loading…
Cancel
Save