Skip to content

Q
qgroundcontrol
Commit 19a80ae1 authored by Don Gagne's avatar Don Gagne Committed by GitHub
Browse files
Options

Merge pull request #4306 from DonLakeFlyer/CorrectMavlinkHandling 

Correct message handling: GPS_RAW_INT, GLOBAL_POSITION_INT, VFR_HUD, ALTITUDE
parents 23d3eb50 004bda50
Hide whitespace changes
Inline Side-by-side
Showing with 95 additions and 662 deletions
src/FirmwarePlugin/APM/ArduCopterFirmwarePlugin.cc
View file @ 19a80ae1
......@@ -120,16 +120,11 @@ void ArduCopterFirmwarePlugin::guidedModeLand(Vehicle* vehicle)
void ArduCopterFirmwarePlugin::guidedModeTakeoff(Vehicle* vehicle, double altitudeRel)
{
if (qIsNaN(vehicle->altitudeAMSL()->rawValue().toDouble())) {
qgcApp()->showMessage(QStringLiteral("Unable to takeoff, vehicle position not known."));
return;
}
vehicle->sendMavCommand(vehicle->defaultComponentId(),
MAV_CMD_NAV_TAKEOFF,
true, // show error
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
vehicle->altitudeAMSL()->rawValue().toFloat() + altitudeRel); // AMSL meters
altitudeRel);
}
void ArduCopterFirmwarePlugin::guidedModeGotoLocation(Vehicle* vehicle, const QGeoCoordinate& gotoCoord)
......
src/FlightDisplay/FlightDisplayViewWidgets.qml
View file @ 19a80ae1
......@@ -437,7 +437,7 @@ Item {
break;
case confirmChangeAlt:
altitudeSlider.visible = true
altitudeSlider.setInitialValueAppSettingsDistanceUnits(_activeVehicle.altitudeAMSL.value)
altitudeSlider.setInitialValueAppSettingsDistanceUnits(_activeVehicle.altitudeRelative.value)
guidedModeConfirm.confirmText = qsTr("change altitude")
break;
case confirmGoTo:
......
src/Vehicle/GPSFact.json
View file @ 19a80ae1
......@@ -29,7 +29,6 @@
{
"name": "count",
"shortDescription": "Sat Count",
"type": "double",
"decimalPlaces": 0
"type": "uint32"
}
]
src/Vehicle/Vehicle.cc
View file @ 19a80ae1
......@@ -85,11 +85,6 @@ Vehicle::Vehicle(LinkInterface* link,
, _currentWarningCount(0)
, _currentNormalCount(0)
, _currentMessageType(MessageNone)
, _navigationAltitudeError(0.0f)
, _navigationSpeedError(0.0f)
, _navigationCrosstrackError(0.0f)
, _navigationTargetBearing(0.0f)
, _refreshTimer(new QTimer(this))
, _updateCount(0)
, _rcRSSI(255)
, _rcRSSIstore(255)
......@@ -99,6 +94,9 @@ Vehicle::Vehicle(LinkInterface* link,
, _onboardControlSensorsEnabled(0)
, _onboardControlSensorsHealth(0)
, _onboardControlSensorsUnhealthy(0)
, _useGpsRawIntForPosition(true)
, _useGpsRawIntForAltitude(true)
, _useAltitudeForAltitude(false)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _missionManager(NULL)
......@@ -150,11 +148,6 @@ Vehicle::Vehicle(LinkInterface* link,
connect(this, &Vehicle::armedChanged, this, &Vehicle::_announceArmedChanged);
_uas = new UAS(_mavlink, this, _firmwarePluginManager);
setLatitude(_uas->getLatitude());
setLongitude(_uas->getLongitude());
connect(_uas, &UAS::latitudeChanged, this, &Vehicle::setLatitude);
connect(_uas, &UAS::longitudeChanged, this, &Vehicle::setLongitude);
connect(_uas, &UAS::imageReady, this, &Vehicle::_imageReady);
connect(this, &Vehicle::remoteControlRSSIChanged, this, &Vehicle::_remoteControlRSSIChanged);
......@@ -164,11 +157,6 @@ Vehicle::Vehicle(LinkInterface* link,
// connect this vehicle to the follow me handle manager
connect(this, &Vehicle::flightModeChanged,qgcApp()->toolbox()->followMe(), &FollowMe::followMeHandleManager);
// Refresh timer
connect(_refreshTimer, &QTimer::timeout, this, &Vehicle::_checkUpdate);
_refreshTimer->setInterval(UPDATE_TIMER);
_refreshTimer->start(UPDATE_TIMER);
// PreArm Error self-destruct timer
connect(&_prearmErrorTimer, &QTimer::timeout, this, &Vehicle::_prearmErrorTimeout);
_prearmErrorTimer.setInterval(_prearmErrorTimeoutMSecs);
......@@ -196,11 +184,6 @@ Vehicle::Vehicle(LinkInterface* link,
connect(_mav, SIGNAL(attitudeChanged (UASInterface*,int,double,double,double,quint64)), this, SLOT(_updateAttitude(UASInterface*,int,double, double, double, quint64)));
connect(_mav, SIGNAL(statusChanged (UASInterface*,QString,QString)), this, SLOT(_updateState(UASInterface*, QString,QString)));
connect(_mav, &UASInterface::speedChanged, this, &Vehicle::_updateSpeed);
connect(_mav, &UASInterface::altitudeChanged, this, &Vehicle::_updateAltitude);
connect(_mav, &UASInterface::navigationControllerErrorsChanged,this, &Vehicle::_updateNavigationControllerErrors);
connect(_mav, &UASInterface::NavigationControllerDataChanged, this, &Vehicle::_updateNavigationControllerData);
_loadSettings();
_missionManager = new MissionManager(this);
......@@ -284,11 +267,6 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _currentWarningCount(0)
, _currentNormalCount(0)
, _currentMessageType(MessageNone)
, _navigationAltitudeError(0.0f)
, _navigationSpeedError(0.0f)
, _navigationCrosstrackError(0.0f)
, _navigationTargetBearing(0.0f)
, _refreshTimer(new QTimer(this))
, _updateCount(0)
, _rcRSSI(255)
, _rcRSSIstore(255)
......@@ -298,6 +276,9 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _onboardControlSensorsEnabled(0)
, _onboardControlSensorsHealth(0)
, _onboardControlSensorsUnhealthy(0)
, _useGpsRawIntForPosition(true)
, _useGpsRawIntForAltitude(true)
, _useAltitudeForAltitude(false)
, _connectionLost(false)
, _connectionLostEnabled(true)
, _missionManager(NULL)
......@@ -496,6 +477,18 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_LOGGING_DATA_ACKED:
_handleMavlinkLoggingDataAcked(message);
break;
case MAVLINK_MSG_ID_GPS_RAW_INT:
_handleGpsRawInt(message);
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
_handleGlobalPositionInt(message);
break;
case MAVLINK_MSG_ID_ALTITUDE:
_handleAltitude(message);
break;
case MAVLINK_MSG_ID_VFR_HUD:
_handleVfrHud(message);
break;
// Following are ArduPilot dialect messages
......@@ -509,6 +502,70 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
_uas->receiveMessage(message);
}
void Vehicle::_handleVfrHud(mavlink_message_t& message)
{
mavlink_vfr_hud_t vfrHud;
mavlink_msg_vfr_hud_decode(&message, &vfrHud);
_airSpeedFact.setRawValue(qIsNaN(vfrHud.airspeed) ? 0 : vfrHud.airspeed);
_groundSpeedFact.setRawValue(qIsNaN(vfrHud.groundspeed) ? 0 : vfrHud.groundspeed);
_climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb);
}
void Vehicle::_handleGpsRawInt(mavlink_message_t& message)
{
mavlink_gps_raw_int_t gpsRawInt;
mavlink_msg_gps_raw_int_decode(&message, &gpsRawInt);
if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
if (_useGpsRawIntForPosition) {
setLatitude(gpsRawInt.lat / (double)1E7);
setLongitude(gpsRawInt.lon / (double)1E7);
}
if (_useGpsRawIntForAltitude) {
_altitudeRelativeFact.setRawValue(gpsRawInt.alt / 1000.0);
}
}
_gpsFactGroup.count()->setRawValue(gpsRawInt.satellites_visible == 255 ? 0 : gpsRawInt.satellites_visible);
_gpsFactGroup.hdop()->setRawValue(gpsRawInt.eph == UINT16_MAX ? 1e10f : gpsRawInt.eph / 100.0);
_gpsFactGroup.vdop()->setRawValue(gpsRawInt.epv == UINT16_MAX ? 1e10f : gpsRawInt.epv / 100.0);
_gpsFactGroup.courseOverGround()->setRawValue(gpsRawInt.cog == UINT16_MAX ? 0.0 : gpsRawInt.cog / 100.0);
_gpsFactGroup.lock()->setRawValue(gpsRawInt.fix_type);
if (gpsRawInt.fix_type >= GPS_FIX_TYPE_3D_FIX) {
_setCoordinateValid(true);
}
}
void Vehicle::_handleGlobalPositionInt(mavlink_message_t& message)
{
mavlink_global_position_int_t globalPositionInt;
mavlink_msg_global_position_int_decode(&message, &globalPositionInt);
_useGpsRawIntForPosition = false;
_useGpsRawIntForAltitude = false;
setLatitude(globalPositionInt.lat / (double)1E7);
setLongitude(globalPositionInt.lon / (double)1E7);
if (!_useAltitudeForAltitude) {
_altitudeRelativeFact.setRawValue(globalPositionInt.relative_alt / 1000.0);
_altitudeAMSLFact.setRawValue(globalPositionInt.alt / 1000.0);
}
}
void Vehicle::_handleAltitude(mavlink_message_t& message)
{
mavlink_altitude_t altitude;
mavlink_msg_altitude_decode(&message, &altitude);
_useAltitudeForAltitude = true;
_useGpsRawIntForAltitude = false;
_altitudeRelativeFact.setRawValue(altitude.altitude_relative / 1000.0);
_altitudeAMSLFact.setRawValue(altitude.altitude_amsl / 1000.0);
}
void Vehicle::_handleAutopilotVersion(LinkInterface *link, mavlink_message_t& message)
{
mavlink_autopilot_version_t autopilotVersion;
......@@ -986,31 +1043,6 @@ void Vehicle::_updateAttitude(UASInterface* uas, int, double roll, double pitch,
_updateAttitude(uas, roll, pitch, yaw, timestamp);
}
void Vehicle::_updateSpeed(UASInterface*, double groundSpeed, double airSpeed, quint64)
{
_groundSpeedFact.setRawValue(groundSpeed);
_airSpeedFact.setRawValue(airSpeed);
}
void Vehicle::_updateAltitude(UASInterface*, double altitudeAMSL, double altitudeRelative, double climbRate, quint64)
{
_altitudeAMSLFact.setRawValue(altitudeAMSL);
_altitudeRelativeFact.setRawValue(altitudeRelative);
_climbRateFact.setRawValue(climbRate);
}
void Vehicle::_updateNavigationControllerErrors(UASInterface*, double altitudeError, double speedError, double xtrackError) {
_navigationAltitudeError = altitudeError;
_navigationSpeedError = speedError;
_navigationCrosstrackError = xtrackError;
}
void Vehicle::_updateNavigationControllerData(UASInterface *uas, float, float, float, float targetBearing, float) {
if (_mav == uas) {
_navigationTargetBearing = targetBearing;
}
}
int Vehicle::motorCount(void)
{
switch (_vehicleType) {
......@@ -1046,19 +1078,6 @@ bool Vehicle::xConfigMotors(void)
* Internal
*/
void Vehicle::_checkUpdate()
{
// Update current location
if(_mav) {
if(latitude() != _mav->getLatitude()) {
setLatitude(_mav->getLatitude());
}
if(longitude() != _mav->getLongitude()) {
setLongitude(_mav->getLongitude());
}
}
}
QString Vehicle::getMavIconColor()
{
// TODO: Not using because not only the colors are ghastly, it doesn't respect dark/light palette
......@@ -2115,53 +2134,6 @@ void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData)
void VehicleGPSFactGroup::setVehicle(Vehicle* vehicle)
{
_vehicle = vehicle;
if (!vehicle) {
// Disconnected Vehicle
return;
}
connect(_vehicle->uas(), &UASInterface::localizationChanged, this, &VehicleGPSFactGroup::_setSatLoc);
UAS* pUas = dynamic_cast<UAS*>(_vehicle->uas());
connect(pUas, &UAS::satelliteCountChanged, this, &VehicleGPSFactGroup::_setSatelliteCount);
connect(pUas, &UAS::satRawHDOPChanged, this, &VehicleGPSFactGroup::_setSatRawHDOP);
connect(pUas, &UAS::satRawVDOPChanged, this, &VehicleGPSFactGroup::_setSatRawVDOP);
connect(pUas, &UAS::satRawCOGChanged, this, &VehicleGPSFactGroup::_setSatRawCOG);
}
void VehicleGPSFactGroup::_setSatelliteCount(double val, QString)
{
// I'm assuming that a negative value or over 99 means there is no GPS
if(val < 0.0) val = -1.0;
if(val > 99.0) val = -1.0;
_countFact.setRawValue(val);
}
void VehicleGPSFactGroup::_setSatRawHDOP(double val)
{
_hdopFact.setRawValue(val);
}
void VehicleGPSFactGroup::_setSatRawVDOP(double val)
{
_vdopFact.setRawValue(val);
}
void VehicleGPSFactGroup::_setSatRawCOG(double val)
{
_courseOverGroundFact.setRawValue(val);
}
void VehicleGPSFactGroup::_setSatLoc(UASInterface*, int fix)
{
_lockFact.setRawValue(fix);
// fix 0: lost, 1: at least one satellite, but no GPS fix, 2: 2D lock, 3: 3D lock
if (fix > 2) {
_vehicle->_setCoordinateValid(true);
}
}
const char* VehicleBatteryFactGroup::_voltageFactName = "voltage";
......
src/Vehicle/Vehicle.h
View file @ 19a80ae1
......@@ -137,13 +137,6 @@ public:
static const char* _countFactName;
static const char* _lockFactName;
private slots:
void _setSatelliteCount(double val, QString);
void _setSatRawHDOP(double val);
void _setSatRawVDOP(double val);
void _setSatRawCOG(double val);
void _setSatLoc(UASInterface*, int fix);
private:
Vehicle* _vehicle;
Fact _hdopFact;
......@@ -683,11 +676,6 @@ private slots:
void _updateAttitude (UASInterface* uas, double roll, double pitch, double yaw, quint64 timestamp);
/** @brief Attitude from one specific component / redundant autopilot */
void _updateAttitude (UASInterface* uas, int component, double roll, double pitch, double yaw, quint64 timestamp);
void _updateSpeed (UASInterface* uas, double _groundSpeed, double _airSpeed, quint64 timestamp);
void _updateAltitude (UASInterface* uas, double _altitudeAMSL, double _altitudeRelative, double _climbRate, quint64 timestamp);
void _updateNavigationControllerErrors (UASInterface* uas, double altitudeError, double speedError, double xtrackError);
void _updateNavigationControllerData (UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDistance);
void _checkUpdate ();
void _updateState (UASInterface* system, QString name, QString description);
/** @brief A new camera image has arrived */
void _imageReady (UASInterface* uas);
......@@ -716,6 +704,10 @@ private:
void _handleCommandAck(mavlink_message_t& message);
void _handleAutopilotVersion(LinkInterface* link, mavlink_message_t& message);
void _handleHilActuatorControls(mavlink_message_t& message);
void _handleGpsRawInt(mavlink_message_t& message);
void _handleGlobalPositionInt(mavlink_message_t& message);
void _handleAltitude(mavlink_message_t& message);
void _handleVfrHud(mavlink_message_t& message);
void _missionManagerError(int errorCode, const QString& errorMsg);
void _geoFenceManagerError(int errorCode, const QString& errorMsg);
void _rallyPointManagerError(int errorCode, const QString& errorMsg);
......@@ -763,11 +755,6 @@ private:
int _currentNormalCount;
MessageType_t _currentMessageType;
QString _latestError;
float _navigationAltitudeError;
float _navigationSpeedError;
float _navigationCrosstrackError;
float _navigationTargetBearing;
QTimer* _refreshTimer;
QString _currentState;
int _updateCount;
QString _formatedMessage;
......@@ -779,6 +766,9 @@ private:
uint32_t _onboardControlSensorsEnabled;
uint32_t _onboardControlSensorsHealth;
uint32_t _onboardControlSensorsUnhealthy;
bool _useGpsRawIntForPosition;
bool _useGpsRawIntForAltitude;
bool _useAltitudeForAltitude;
typedef struct {
int component;
......
src/uas/UAS.cc
View file @ 19a80ae1
......@@ -7,14 +7,7 @@
*
****************************************************************************/
/**
* @file
* @brief Represents one unmanned aerial vehicle
*
* @author Lorenz Meier <mavteam@student.ethz.ch>
*
*/
// THIS CLASS IS DEPRECATED. ALL NEW FUNCTIONALITY SHOULD GO INTO Vehicle class
#include <QList>
#include <QTimer>
......@@ -47,13 +40,7 @@
QGC_LOGGING_CATEGORY(UASLog, "UASLog")
/**
* Gets the settings from the previous UAS (name, airframe, autopilot, battery specs)
* by calling readSettings. This means the new UAS will have the same settings
* as the previous one created unless one calls deleteSettings in the code after
* creating the UAS.
*/
// THIS CLASS IS DEPRECATED. ALL NEW FUNCTIONALITY SHOULD GO INTO Vehicle class
UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * firmwarePluginManager) : UASInterface(),
lipoFull(4.2f),
lipoEmpty(3.5f),
......@@ -77,30 +64,6 @@ UAS::UAS(MAVLinkProtocol* protocol, Vehicle* vehicle, FirmwarePluginManager * fi
manualYawAngle(0),
manualThrust(0),
isGlobalPositionKnown(false),
latitude(0.0),
longitude(0.0),
altitudeAMSL(0.0),
altitudeAMSLFT(0.0),
altitudeRelative(0.0),
satRawHDOP(1e10f),
satRawVDOP(1e10f),
satRawCOG(0.0),
globalEstimatorActive(false),
latitude_gps(0.0),
longitude_gps(0.0),
altitude_gps(0.0),
speedX(0.0),
speedY(0.0),
speedZ(0.0),
airSpeed(std::numeric_limits<double>::quiet_NaN()),
groundSpeed(std::numeric_limits<double>::quiet_NaN()),
#ifndef __mobile__
fileManager(this, vehicle),
#endif
......@@ -283,14 +246,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit valueChanged(uasId, name.arg("custom_mode"), "bits", state.custom_mode, time);
emit valueChanged(uasId, name.arg("system_status"), "-", state.system_status, time);
if ((state.system_status != this->status) && state.system_status != MAV_STATE_UNINIT)
{
this->status = state.system_status;
getStatusForCode((int)state.system_status, uasState, stateDescription);
emit statusChanged(this, uasState, stateDescription);
emit statusChanged(this->status);
}
// We got the mode
receivedMode = true;
}
......@@ -318,25 +273,7 @@ void UAS::receiveMessage(mavlink_message_t message)
emit valueChanged(uasId, name.arg("errors_count4"), "-", state.errors_count4, time);
// Process CPU load.
emit loadChanged(this,state.load/10.0f);
emit valueChanged(uasId, name.arg("load"), "%", state.load/10.0f, time);
// control_sensors_enabled:
// relevant bits: 11: attitude stabilization, 12: yaw position, 13: z/altitude control, 14: x/y position control
emit attitudeControlEnabled(state.onboard_control_sensors_enabled & (1 << 11));
emit positionYawControlEnabled(state.onboard_control_sensors_enabled & (1 << 12));
emit positionZControlEnabled(state.onboard_control_sensors_enabled & (1 << 13));
emit positionXYControlEnabled(state.onboard_control_sensors_enabled & (1 << 14));
// Trigger drop rate updates as needed. Here we convert the incoming
// drop_rate_comm value from 1/100 of a percent in a uint16 to a true
// percentage as a float. We also cap the incoming value at 100% as defined
// by the MAVLink specifications.
if (state.drop_rate_comm > 10000)
{
state.drop_rate_comm = 10000;
}
emit dropRateChanged(this->getUASID(), state.drop_rate_comm/100.0f);
emit valueChanged(uasId, name.arg("drop_rate_comm"), "%", state.drop_rate_comm/100.0f, time);
}
break;
......@@ -357,7 +294,6 @@ void UAS::receiveMessage(mavlink_message_t message)
attitudeKnown = true;
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
}
}
break;
......@@ -418,7 +354,6 @@ void UAS::receiveMessage(mavlink_message_t message)
attitudeKnown = true;
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
emit attitudeRotationRatesChanged(uasId, attitude.rollspeed, attitude.pitchspeed, attitude.yawspeed, time);
}
}
break;
......@@ -434,41 +369,12 @@ void UAS::receiveMessage(mavlink_message_t message)
mavlink_vfr_hud_t hud;
mavlink_msg_vfr_hud_decode(&message, &hud);
quint64 time = getUnixTime();
// Display updated values
emit thrustChanged(this, hud.throttle/100.0);
if (!attitudeKnown)
{
setYaw(QGC::limitAngleToPMPId((((double)hud.heading)/180.0)*M_PI));
emit attitudeChanged(this, getRoll(), getPitch(), getYaw(), time);
}
setAltitudeAMSL(hud.alt);
setGroundSpeed(hud.groundspeed);
if (!qIsNaN(hud.airspeed))
setAirSpeed(hud.airspeed);
speedZ = -hud.climb;
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
emit speedChanged(this, groundSpeed, airSpeed, time);
}
break;
case MAVLINK_MSG_ID_LOCAL_POSITION_NED:
//std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
{
mavlink_local_position_ned_t pos;
mavlink_msg_local_position_ned_decode(&message, &pos);
quint64 time = getUnixTime(pos.time_boot_ms);
if (!wrongComponent)
{
speedX = pos.vx;
speedY = pos.vy;
speedZ = pos.vz;
// Emit
emit velocityChanged_NED(this, speedX, speedY, speedZ, time);
}
}
break;
case MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE:
......@@ -479,114 +385,6 @@ void UAS::receiveMessage(mavlink_message_t message)
emit attitudeChanged(this, message.compid, pos.roll, pos.pitch, pos.yaw, time);
}
break;
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
//std::cerr << std::endl;
//std::cerr << "Decoded attitude message:" << " roll: " << std::dec << mavlink_msg_attitude_get_roll(message.payload) << " pitch: " << mavlink_msg_attitude_get_pitch(message.payload) << " yaw: " << mavlink_msg_attitude_get_yaw(message.payload) << std::endl;
{
mavlink_global_position_int_t pos;
mavlink_msg_global_position_int_decode(&message, &pos);
quint64 time = getUnixTime();
setLatitude(pos.lat/(double)1E7);
setLongitude(pos.lon/(double)1E7);
setAltitudeRelative(pos.relative_alt/1000.0);
globalEstimatorActive = true;
speedX = pos.vx/100.0;
speedY = pos.vy/100.0;
speedZ = pos.vz/100.0;
emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time);
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
// We had some frame mess here, global and local axes were mixed.
emit velocityChanged_NED(this, speedX, speedY, speedZ, time);
setGroundSpeed(qSqrt(speedX*speedX+speedY*speedY));
emit speedChanged(this, groundSpeed, airSpeed, time);
isGlobalPositionKnown = true;
}
break;
case MAVLINK_MSG_ID_GPS_RAW_INT:
{
mavlink_gps_raw_int_t pos;
mavlink_msg_gps_raw_int_decode(&message, &pos);
quint64 time = getUnixTime(pos.time_usec);
// TODO: track localization state not only for gps but also for other loc. sources
int loc_type = pos.fix_type;
if (loc_type == 1)
{
loc_type = 0;
}
setSatelliteCount(pos.satellites_visible);
if (pos.fix_type > 2)
{
isGlobalPositionKnown = true;
latitude_gps = pos.lat/(double)1E7;
longitude_gps = pos.lon/(double)1E7;
altitude_gps = pos.alt/1000.0;
// If no GLOBAL_POSITION_INT messages ever received, use these raw GPS values instead.
if (!globalEstimatorActive) {
setLatitude(latitude_gps);
setLongitude(longitude_gps);
emit globalPositionChanged(this, getLatitude(), getLongitude(), getAltitudeAMSL(), time);
emit altitudeChanged(this, altitudeAMSL, altitudeRelative, -speedZ, time);
float vel = pos.vel/100.0f;
// Smaller than threshold and not NaN
if ((vel < 1000000) && !qIsNaN(vel) && !qIsInf(vel)) {
setGroundSpeed(vel);
emit speedChanged(this, groundSpeed, airSpeed, time);
} else {
emit textMessageReceived(uasId, message.compid, MAV_SEVERITY_NOTICE, QString("GCS ERROR: RECEIVED INVALID SPEED OF %1 m/s").arg(vel));
}
}
}
double dtmp;
//-- Raw GPS data
dtmp = pos.eph == 0xFFFF ? 1e10f : pos.eph / 100.0;
if(dtmp != satRawHDOP)
{
satRawHDOP = dtmp;
emit satRawHDOPChanged(satRawHDOP);
}
dtmp = pos.epv == 0xFFFF ? 1e10f : pos.epv / 100.0;
if(dtmp != satRawVDOP)
{
satRawVDOP = dtmp;
emit satRawVDOPChanged(satRawVDOP);
}
dtmp = pos.cog == 0xFFFF ? 0.0 : pos.cog / 100.0;
if(dtmp != satRawCOG)
{
satRawCOG = dtmp;
emit satRawCOGChanged(satRawCOG);
}
// Emit this signal after the above signals. This way a trigger on gps lock signal which then asks for vehicle position
// gets a good position.
emit localizationChanged(this, loc_type);
}
break;
case MAVLINK_MSG_ID_GPS_STATUS:
{
mavlink_gps_status_t pos;
mavlink_msg_gps_status_decode(&message, &pos);
for(int i = 0; i < (int)pos.satellites_visible; i++)
{
emit gpsSatelliteStatusChanged(uasId, (unsigned char)pos.satellite_prn[i], (unsigned char)pos.satellite_elevation[i], (unsigned char)pos.satellite_azimuth[i], (unsigned char)pos.satellite_snr[i], static_cast<bool>(pos.satellite_used[i]));
}
setSatelliteCount(pos.satellites_visible);
}
break;
case MAVLINK_MSG_ID_PARAM_VALUE:
{
......@@ -610,7 +408,6 @@ void UAS::receiveMessage(mavlink_message_t message)
float roll, pitch, yaw;
mavlink_quaternion_to_euler(out.q, &roll, &pitch, &yaw);
quint64 time = getUnixTimeFromMs(out.time_boot_ms);
emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, out.thrust, time);
// For plotting emit roll sp, pitch sp and yaw sp values
emit valueChanged(uasId, "roll sp", "rad", roll, time);
......@@ -619,25 +416,6 @@ void UAS::receiveMessage(mavlink_message_t message)
}
break;
case MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED:
{
if (multiComponentSourceDetected && wrongComponent)
{
break;
}
mavlink_position_target_local_ned_t p;
mavlink_msg_position_target_local_ned_decode(&message, &p);
quint64 time = getUnixTimeFromMs(p.time_boot_ms);
emit positionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */, time);
}
break;
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
{
mavlink_set_position_target_local_ned_t p;
mavlink_msg_set_position_target_local_ned_decode(&message, &p);
emit userPositionSetPointsChanged(uasId, p.x, p.y, p.z, 0/* XXX remove yaw and move it to attitude */);
}
break;
case MAVLINK_MSG_ID_STATUSTEXT:
{
QByteArray b;
......@@ -718,17 +496,6 @@ void UAS::receiveMessage(mavlink_message_t message)
}
break;
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
{
mavlink_nav_controller_output_t p;
mavlink_msg_nav_controller_output_decode(&message,&p);
setDistToWaypoint(p.wp_dist);
setBearingToWaypoint(p.nav_bearing);
emit navigationControllerErrorsChanged(this, p.alt_error, p.aspd_error, p.xtrack_error);
emit NavigationControllerDataChanged(this, p.nav_roll, p.nav_pitch, p.nav_bearing, p.target_bearing, p.wp_dist);
}
break;
case MAVLINK_MSG_ID_LOG_ENTRY:
{
mavlink_log_entry_t log;
......@@ -1388,8 +1155,6 @@ void UAS::setExternalControlSetpoint(float roll, float pitch, float yaw, float t
}
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
// Emit an update in control values to other UI elements, like the HSI display
emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
}
}
......@@ -1428,8 +1193,6 @@ void UAS::setManual6DOFControlCommands(double x, double y, double z, double roll
MAV_FRAME_LOCAL_NED, position_mask, x, y, z, 0, 0, 0, 0, 0, 0, yaw, yawrate);
_vehicle->sendMessageOnLink(_vehicle->priorityLink(), message);
qDebug() << __FILE__ << __LINE__ << ": SENT 6DOF CONTROL MESSAGES: x" << x << " y: " << y << " z: " << z << " roll: " << roll << " pitch: " << pitch << " yaw: " << yaw;
//emit attitudeThrustSetPointChanged(this, roll, pitch, yaw, thrust, QGC::groundTimeMilliseconds());
}
else
{
......
src/uas/UAS.h
View file @ 19a80ae1
......@@ -65,197 +65,13 @@ public:
/** @brief The time interval the robot is switched on */
quint64 getUptime() const;
Q_PROPERTY(double latitude READ getLatitude WRITE setLatitude NOTIFY latitudeChanged)
Q_PROPERTY(double longitude READ getLongitude WRITE setLongitude NOTIFY longitudeChanged)
Q_PROPERTY(double satelliteCount READ getSatelliteCount WRITE setSatelliteCount NOTIFY satelliteCountChanged)
Q_PROPERTY(bool isGlobalPositionKnown READ globalPositionKnown)
Q_PROPERTY(double roll READ getRoll WRITE setRoll NOTIFY rollChanged)
Q_PROPERTY(double pitch READ getPitch WRITE setPitch NOTIFY pitchChanged)
Q_PROPERTY(double yaw READ getYaw WRITE setYaw NOTIFY yawChanged)
Q_PROPERTY(double distToWaypoint READ getDistToWaypoint WRITE setDistToWaypoint NOTIFY distToWaypointChanged)
Q_PROPERTY(double airSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY airSpeedChanged)
Q_PROPERTY(double groundSpeed READ getGroundSpeed WRITE setGroundSpeed NOTIFY groundSpeedChanged)
Q_PROPERTY(double bearingToWaypoint READ getBearingToWaypoint WRITE setBearingToWaypoint NOTIFY bearingToWaypointChanged)
Q_PROPERTY(double altitudeAMSL READ getAltitudeAMSL WRITE setAltitudeAMSL NOTIFY altitudeAMSLChanged)
Q_PROPERTY(double altitudeAMSLFT READ getAltitudeAMSLFT NOTIFY altitudeAMSLFTChanged)
Q_PROPERTY(double altitudeRelative READ getAltitudeRelative WRITE setAltitudeRelative NOTIFY altitudeRelativeChanged)
Q_PROPERTY(double satRawHDOP READ getSatRawHDOP NOTIFY satRawHDOPChanged)
Q_PROPERTY(double satRawVDOP READ getSatRawVDOP NOTIFY satRawVDOPChanged)
Q_PROPERTY(double satRawCOG READ getSatRawCOG NOTIFY satRawCOGChanged)
/// Vehicle is about to go away
void shutdownVehicle(void);
void setGroundSpeed(double val)
{
groundSpeed = val;
emit groundSpeedChanged(val,"groundSpeed");
emit valueChanged(this->uasId,"groundSpeed","m/s",QVariant(val),getUnixTime());
}
double getGroundSpeed() const
{
return groundSpeed;
}
void setAirSpeed(double val)
{
airSpeed = val;
emit airSpeedChanged(val,"airSpeed");
emit valueChanged(this->uasId,"airSpeed","m/s",QVariant(val),getUnixTime());
}
double getAirSpeed() const
{
return airSpeed;
}
void setLocalX(double val)
{
localX = val;
emit localXChanged(val,"localX");
emit valueChanged(this->uasId,"localX","m",QVariant(val),getUnixTime());
}
double getLocalX() const
{
return localX;
}
void setLocalY(double val)
{
localY = val;
emit localYChanged(val,"localY");
emit valueChanged(this->uasId,"localY","m",QVariant(val),getUnixTime());
}
double getLocalY() const
{
return localY;
}
void setLocalZ(double val)
{
localZ = val;
emit localZChanged(val,"localZ");
emit valueChanged(this->uasId,"localZ","m",QVariant(val),getUnixTime());
}
double getLocalZ() const
{
return localZ;
}
void setLatitude(double val)
{
latitude = val;
emit latitudeChanged(val,"latitude");
emit valueChanged(this->uasId,"latitude","deg",QVariant(val),getUnixTime());
}
double getLatitude() const
{
return latitude;
}
void setLongitude(double val)
{
longitude = val;
emit longitudeChanged(val,"longitude");
emit valueChanged(this->uasId,"longitude","deg",QVariant(val),getUnixTime());
}
double getLongitude() const
{
return longitude;
}
void setAltitudeAMSL(double val)
{
altitudeAMSL = val;
emit altitudeAMSLChanged(val, "altitudeAMSL");
emit valueChanged(this->uasId,"altitudeAMSL","m",QVariant(altitudeAMSL),getUnixTime());
altitudeAMSLFT = 3.28084 * altitudeAMSL;
emit altitudeAMSLFTChanged(val, "altitudeAMSLFT");
emit valueChanged(this->uasId,"altitudeAMSLFT","m",QVariant(altitudeAMSLFT),getUnixTime());
}
double getAltitudeAMSL() const
{
return altitudeAMSL;
}
double getAltitudeAMSLFT() const
{
return altitudeAMSLFT;
}
void setAltitudeRelative(double val)
{
altitudeRelative = val;
emit altitudeRelativeChanged(val, "altitudeRelative");
emit valueChanged(this->uasId,"altitudeRelative","m",QVariant(val),getUnixTime());
}
double getAltitudeRelative() const
{
return altitudeRelative;
}
double getSatRawHDOP() const
{
return satRawHDOP;
}
double getSatRawVDOP() const
{
return satRawVDOP;
}
double getSatRawCOG() const
{
return satRawCOG;
}
void setSatelliteCount(double val)
{
satelliteCount = val;
emit satelliteCountChanged(val,"satelliteCount");
emit valueChanged(this->uasId,"satelliteCount","",QVariant(val),getUnixTime());
}
double getSatelliteCount() const
{
return satelliteCount;
}
virtual bool globalPositionKnown() const
{
return isGlobalPositionKnown;
}
void setDistToWaypoint(double val)
{
distToWaypoint = val;
emit distToWaypointChanged(val,"distToWaypoint");
emit valueChanged(this->uasId,"distToWaypoint","m",QVariant(val),getUnixTime());
}
double getDistToWaypoint() const
{
return distToWaypoint;
}
void setBearingToWaypoint(double val)
{
bearingToWaypoint = val;
emit bearingToWaypointChanged(val,"bearingToWaypoint");
emit valueChanged(this->uasId,"bearingToWaypoint","deg",QVariant(val),getUnixTime());
}
double getBearingToWaypoint() const
{
return bearingToWaypoint;
}
void setRoll(double val)
{
roll = val;
......@@ -377,34 +193,6 @@ protected: //COMMENTS FOR TEST UNIT
/// POSITION
bool isGlobalPositionKnown; ///< If the global position has been received for this MAV
double localX;
double localY;
double localZ;
double latitude; ///< Global latitude as estimated by position estimator
double longitude; ///< Global longitude as estimated by position estimator
double altitudeAMSL; ///< Global altitude as estimated by position estimator, AMSL
double altitudeAMSLFT; ///< Global altitude as estimated by position estimator, AMSL
double altitudeRelative; ///< Altitude above home as estimated by position estimator
double satRawHDOP;
double satRawVDOP;
double satRawCOG;
double satelliteCount; ///< Number of satellites visible to raw GPS
bool globalEstimatorActive; ///< Global position estimator present, do not fall back to GPS raw for position
double latitude_gps; ///< Global latitude as estimated by raw GPS
double longitude_gps; ///< Global longitude as estimated by raw GPS
double altitude_gps; ///< Global altitude as estimated by raw GPS
double speedX; ///< True speed in X axis
double speedY; ///< True speed in Y axis
double speedZ; ///< True speed in Z axis
/// WAYPOINT NAVIGATION
double distToWaypoint; ///< Distance to next waypoint
double airSpeed; ///< Airspeed
double groundSpeed; ///< Groundspeed
double bearingToWaypoint; ///< Bearing to next waypoint
#ifndef __mobile__
FileManager fileManager;
#endif
......@@ -545,34 +333,16 @@ public slots:
/** @brief Send command to disable all bindings/maps between RC and parameters */
void unsetRCToParameterMap();
signals:
void loadChanged(UASInterface* uas, double load);
void imageStarted(quint64 timestamp);
/** @brief A new camera image has arrived */
void imageReady(UASInterface* uas);
/** @brief HIL controls have changed */
void hilControlsChanged(quint64 time, float rollAilerons, float pitchElevator, float yawRudder, float throttle, quint8 systemMode, quint8 navMode);
void localXChanged(double val,QString name);
void localYChanged(double val,QString name);
void localZChanged(double val,QString name);
void longitudeChanged(double val,QString name);
void latitudeChanged(double val,QString name);
void altitudeAMSLChanged(double val,QString name);
void altitudeAMSLFTChanged(double val,QString name);
void altitudeRelativeChanged(double val,QString name);
void satRawHDOPChanged (double value);
void satRawVDOPChanged (double value);
void satRawCOGChanged (double value);
void rollChanged(double val,QString name);
void pitchChanged(double val,QString name);
void yawChanged(double val,QString name);
void satelliteCountChanged(double val,QString name);
void distToWaypointChanged(double val,QString name);
void groundSpeedChanged(double val, QString name);
void airSpeedChanged(double val, QString name);
void bearingToWaypointChanged(double val,QString name);
protected:
/** @brief Get the UNIX timestamp in milliseconds, enter microseconds */
quint64 getUnixTime(quint64 time=0);
......
src/uas/UASInterface.h
View file @ 19a80ae1
......@@ -50,12 +50,6 @@ public:
/** @brief The time interval the robot is switched on **/
virtual quint64 getUptime() const = 0;
virtual double getLatitude() const = 0;
virtual double getLongitude() const = 0;
virtual double getAltitudeAMSL() const = 0;
virtual double getAltitudeRelative() const = 0;
virtual bool globalPositionKnown() const = 0;
virtual double getRoll() const = 0;
virtual double getPitch() const = 0;
virtual double getYaw() const = 0;
......@@ -172,16 +166,6 @@ protected:
QColor color;
signals:
/** @brief The robot state has changed */
void statusChanged(int stateFlag);
/** @brief The robot state has changed
*
* @param uas this robot
* @param status short description of status, e.g. "connected"
* @param description longer textual description. Should be however limited to a short text, e.g. 200 chars.
*/
void statusChanged(UASInterface* uas, QString status, QString description);
/** @brief A text message from the system has been received */
void textMessageReceived(int uasid, int componentid, int severity, QString text);
......@@ -200,13 +184,6 @@ signals:
*/
void errCountChanged(int uasid, QString component, QString device, int count);
/**
* @brief Drop rate of communication link updated
*
* @param systemId id of the air system
* @param receiveDrop drop rate of packets this MAV receives (sent from GCS or other MAVs)
*/
void dropRateChanged(int systemId, float receiveDrop);
/** @brief The robot is connected **/
void connected();
/** @brief The robot is disconnected **/
......@@ -238,39 +215,12 @@ signals:
*/
void batteryChanged(UASInterface* uas, double voltage, double current, double percent, int seconds);
void statusChanged(UASInterface* uas, QString status);
void thrustChanged(UASInterface*, double thrust);
void attitudeChanged(UASInterface*, double roll, double pitch, double yaw, quint64 usec);
void attitudeChanged(UASInterface*, int component, double roll, double pitch, double yaw, quint64 usec);
void attitudeRotationRatesChanged(int uas, double rollrate, double pitchrate, double yawrate, quint64 usec);
void attitudeThrustSetPointChanged(UASInterface*, float rollDesired, float pitchDesired, float yawDesired, float thrustDesired, quint64 usec);
/** @brief The MAV set a new setpoint in the local (not body) NED X, Y, Z frame */
void positionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired, quint64 usec);
/** @brief A user (or an autonomous mission or obstacle avoidance planner) requested to set a new setpoint */
void userPositionSetPointsChanged(int uasid, float xDesired, float yDesired, float zDesired, float yawDesired);
void globalPositionChanged(UASInterface*, double lat, double lon, double altAMSL, quint64 usec);
void altitudeChanged(UASInterface*, double altitudeAMSL, double altitudeRelative, double climbRate, quint64 usec);
/** @brief Update the status of one satellite used for localization */
void gpsSatelliteStatusChanged(int uasid, int satid, float azimuth, float direction, float snr, bool used);
// The horizontal speed (a scalar)
void speedChanged(UASInterface* uas, double groundSpeed, double airSpeed, quint64 usec);
// Consider adding a MAV_FRAME parameter to this; could help specifying what the 3 scalars are.
void velocityChanged_NED(UASInterface*, double vx, double vy, double vz, quint64 usec);
void navigationControllerErrorsChanged(UASInterface*, double altitudeError, double speedError, double xtrackError);
void NavigationControllerDataChanged(UASInterface *uas, float navRoll, float navPitch, float navBearing, float targetBearing, float targetDist);
void imageStarted(int imgid, int width, int height, int depth, int channels);
void imageDataReceived(int imgid, const unsigned char* imageData, int length, int startIndex);
/** @brief Attitude control enabled/disabled */
void attitudeControlEnabled(bool enabled);
/** @brief Position 2D control enabled/disabled */
void positionXYControlEnabled(bool enabled);
/** @brief Altitude control enabled/disabled */
void positionZControlEnabled(bool enabled);
/** @brief Heading control enabled/disabled */
void positionYawControlEnabled(bool enabled);
/** @brief Optical flow status changed */
void opticalFlowStatusChanged(bool supported, bool enabled, bool ok);
/** @brief Vision based localization status changed */
......@@ -288,12 +238,6 @@ signals:
/** @brief Differential pressure / airspeed status changed */
void airspeedStatusChanged(bool supported, bool enabled, bool ok);
/**
* @brief Localization quality changed
* @param fix 0: lost, 1: 2D local position hold, 2: 2D localization, 3: 3D localization
*/
void localizationChanged(UASInterface* uas, int fix);
// ERROR AND STATUS SIGNALS
/** @brief Name of system changed */
void nameChanged(QString newName);
......
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