zhaohuwei
/
QG4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Vehicle: add facts for TECS tuninig (alt+airspeed setpoints)

QGC4.4
Beat Küng 4 years ago committed by Lorenz Meier
parent
339006bf64
commit
3d568b342f
2 changed files with 49 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 34
      src/Vehicle/Vehicle.cc
  2. 15
      src/Vehicle/Vehicle.h

34
src/Vehicle/Vehicle.cc
Unescape View File

@ -75,10 +75,13 @@ const char* Vehicle::_rollRateFactName = "rollRate"; @@ -75,10 +75,13 @@ const char* Vehicle::_rollRateFactName = "rollRate";
const char* Vehicle::_pitchRateFactName = "pitchRate";
const char* Vehicle::_yawRateFactName = "yawRate";
const char* Vehicle::_airSpeedFactName = "airSpeed";
const char* Vehicle::_airSpeedSetpointFactName = "airSpeedSetpoint";
const char* Vehicle::_groundSpeedFactName = "groundSpeed";
const char* Vehicle::_climbRateFactName = "climbRate";
const char* Vehicle::_altitudeRelativeFactName = "altitudeRelative";
const char* Vehicle::_altitudeAMSLFactName = "altitudeAMSL";
const char* Vehicle::_altitudeTuningFactName = "altitudeTuning";
const char* Vehicle::_altitudeTuningSetpointFactName = "altitudeTuningSetpoint";
const char* Vehicle::_flightDistanceFactName = "flightDistance";
const char* Vehicle::_flightTimeFactName = "flightTime";
const char* Vehicle::_distanceToHomeFactName = "distanceToHome";
@ -132,9 +135,12 @@ Vehicle::Vehicle(LinkInterface* link, @@ -132,9 +135,12 @@ Vehicle::Vehicle(LinkInterface* link,
, _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble)
, _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble)
, _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble)
, _airSpeedSetpointFact (0, _airSpeedSetpointFactName, FactMetaData::valueTypeDouble)
, _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble)
, _altitudeRelativeFact (0, _altitudeRelativeFactName, FactMetaData::valueTypeDouble)
, _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble)
, _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble)
, _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble)
, _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble)
, _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds)
, _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble)
@ -282,9 +288,12 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType, @@ -282,9 +288,12 @@ Vehicle::Vehicle(MAV_AUTOPILOT firmwareType,
, _yawRateFact (0, _yawRateFactName, FactMetaData::valueTypeDouble)
, _groundSpeedFact (0, _groundSpeedFactName, FactMetaData::valueTypeDouble)
, _airSpeedFact (0, _airSpeedFactName, FactMetaData::valueTypeDouble)
, _airSpeedSetpointFact (0, _airSpeedSetpointFactName, FactMetaData::valueTypeDouble)
, _climbRateFact (0, _climbRateFactName, FactMetaData::valueTypeDouble)
, _altitudeRelativeFact (0, _altitudeRelativeFactName, FactMetaData::valueTypeDouble)
, _altitudeAMSLFact (0, _altitudeAMSLFactName, FactMetaData::valueTypeDouble)
, _altitudeTuningFact (0, _altitudeTuningFactName, FactMetaData::valueTypeDouble)
, _altitudeTuningSetpointFact (0, _altitudeTuningSetpointFactName, FactMetaData::valueTypeDouble)
, _flightDistanceFact (0, _flightDistanceFactName, FactMetaData::valueTypeDouble)
, _flightTimeFact (0, _flightTimeFactName, FactMetaData::valueTypeElapsedTimeInSeconds)
, _distanceToHomeFact (0, _distanceToHomeFactName, FactMetaData::valueTypeDouble)
@ -391,9 +400,12 @@ void Vehicle::_commonInit() @@ -391,9 +400,12 @@ void Vehicle::_commonInit()
_addFact(&_yawRateFact, _yawRateFactName);
_addFact(&_groundSpeedFact, _groundSpeedFactName);
_addFact(&_airSpeedFact, _airSpeedFactName);
_addFact(&_airSpeedSetpointFact, _airSpeedSetpointFactName);
_addFact(&_climbRateFact, _climbRateFactName);
_addFact(&_altitudeRelativeFact, _altitudeRelativeFactName);
_addFact(&_altitudeAMSLFact, _altitudeAMSLFactName);
_addFact(&_altitudeTuningFact, _altitudeTuningFactName);
_addFact(&_altitudeTuningSetpointFact, _altitudeTuningSetpointFactName);
_addFact(&_flightDistanceFact, _flightDistanceFactName);
_addFact(&_flightTimeFact, _flightTimeFactName);
_addFact(&_distanceToHomeFact, _distanceToHomeFactName);
@ -687,6 +699,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes @@ -687,6 +699,9 @@ void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t mes
case MAVLINK_MSG_ID_VFR_HUD:
_handleVfrHud(message);
break;
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
_handleNavControllerOutput(message);
break;
case MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:
_handleCameraImageCaptured(message);
break;
@ -934,6 +949,19 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message) @@ -934,6 +949,19 @@ void Vehicle::_handleVfrHud(mavlink_message_t& message)
_groundSpeedFact.setRawValue(qIsNaN(vfrHud.groundspeed) ? 0 : vfrHud.groundspeed);
_climbRateFact.setRawValue(qIsNaN(vfrHud.climb) ? 0 : vfrHud.climb);
_throttlePctFact.setRawValue(static_cast<int16_t>(vfrHud.throttle));
if (qIsNaN(_altitudeTuningOffset)) {
_altitudeTuningOffset = vfrHud.alt;
}
_altitudeTuningFact.setRawValue(vfrHud.alt - _altitudeTuningOffset);
}
void Vehicle::_handleNavControllerOutput(mavlink_message_t& message)
{
mavlink_nav_controller_output_t navControllerOutput;
mavlink_msg_nav_controller_output_decode(&message, &navControllerOutput);
_altitudeTuningSetpointFact.setRawValue(_altitudeTuningFact.rawValue().toDouble() - navControllerOutput.alt_error);
_airSpeedSetpointFact.setRawValue(_airSpeedFact.rawValue().toDouble() - navControllerOutput.aspd_error);
}
// Ignore warnings from mavlink headers for both GCC/Clang and MSVC
@ -3575,6 +3603,12 @@ void Vehicle::setPIDTuningTelemetryMode(PIDTuningTelemetryMode mode) @@ -3575,6 +3603,12 @@ void Vehicle::setPIDTuningTelemetryMode(PIDTuningTelemetryMode mode)
break;
case ModeAltitudeAndAirspeed:
_mavlinkStreamConfig.setHighRateAltAirspeed();
// reset the altitude offset to the current value, so the plotted value is around 0
if (!qIsNaN(_altitudeTuningOffset)) {
_altitudeTuningOffset += _altitudeTuningFact.rawValue().toDouble();
_altitudeTuningSetpointFact.setRawValue(0.f);
_altitudeTuningFact.setRawValue(0.f);
}
break;
}
}

15
src/Vehicle/Vehicle.h
Unescape View File

@ -272,9 +272,12 @@ public: @@ -272,9 +272,12 @@ public:
Q_PROPERTY(Fact* yawRate READ yawRate CONSTANT)
Q_PROPERTY(Fact* groundSpeed READ groundSpeed CONSTANT)
Q_PROPERTY(Fact* airSpeed READ airSpeed CONSTANT)
Q_PROPERTY(Fact* airSpeedSetpoint READ airSpeedSetpoint CONSTANT)
Q_PROPERTY(Fact* climbRate READ climbRate CONSTANT)
Q_PROPERTY(Fact* altitudeRelative READ altitudeRelative CONSTANT)
Q_PROPERTY(Fact* altitudeAMSL READ altitudeAMSL CONSTANT)
Q_PROPERTY(Fact* altitudeTuning READ altitudeTuning CONSTANT)
Q_PROPERTY(Fact* altitudeTuningSetpoint READ altitudeTuningSetpoint CONSTANT)
Q_PROPERTY(Fact* flightDistance READ flightDistance CONSTANT)
Q_PROPERTY(Fact* distanceToHome READ distanceToHome CONSTANT)
Q_PROPERTY(Fact* missionItemIndex READ missionItemIndex CONSTANT)
@ -605,10 +608,13 @@ public: @@ -605,10 +608,13 @@ public:
Fact* pitchRate () { return &_pitchRateFact; }
Fact* yawRate () { return &_yawRateFact; }
Fact* airSpeed () { return &_airSpeedFact; }
Fact* airSpeedSetpoint () { return &_airSpeedSetpointFact; }
Fact* groundSpeed () { return &_groundSpeedFact; }
Fact* climbRate () { return &_climbRateFact; }
Fact* altitudeRelative () { return &_altitudeRelativeFact; }
Fact* altitudeAMSL () { return &_altitudeAMSLFact; }
Fact* altitudeTuning () { return &_altitudeTuningFact; }
Fact* altitudeTuningSetpoint () { return &_altitudeTuningSetpointFact; }
Fact* flightDistance () { return &_flightDistanceFact; }
Fact* distanceToHome () { return &_distanceToHomeFact; }
Fact* missionItemIndex () { return &_missionItemIndexFact; }
@ -932,6 +938,7 @@ private: @@ -932,6 +938,7 @@ private:
void _handleGlobalPositionInt (mavlink_message_t& message);
void _handleAltitude (mavlink_message_t& message);
void _handleVfrHud (mavlink_message_t& message);
void _handleNavControllerOutput (mavlink_message_t& message);
void _handleHighLatency (mavlink_message_t& message);
void _handleHighLatency2 (mavlink_message_t& message);
void _handleAttitudeWorker (double rollRadians, double pitchRadians, double yawRadians);
@ -1199,6 +1206,8 @@ private: @@ -1199,6 +1206,8 @@ private:
QMap<uint8_t /* batteryId */, uint8_t /* MAV_BATTERY_CHARGE_STATE_OK */> _lowestBatteryChargeStateAnnouncedMap;
float _altitudeTuningOffset = qQNaN(); // altitude offset, so the plotted value is around 0
// FactGroup facts
Fact _rollFact;
@ -1209,9 +1218,12 @@ private: @@ -1209,9 +1218,12 @@ private:
Fact _yawRateFact;
Fact _groundSpeedFact;
Fact _airSpeedFact;
Fact _airSpeedSetpointFact;
Fact _climbRateFact;
Fact _altitudeRelativeFact;
Fact _altitudeAMSLFact;
Fact _altitudeTuningFact;
Fact _altitudeTuningSetpointFact;
Fact _flightDistanceFact;
Fact _flightTimeFact;
Fact _distanceToHomeFact;
@ -1254,9 +1266,12 @@ private: @@ -1254,9 +1266,12 @@ private:
static const char* _yawRateFactName;
static const char* _groundSpeedFactName;
static const char* _airSpeedFactName;
static const char* _airSpeedSetpointFactName;
static const char* _climbRateFactName;
static const char* _altitudeRelativeFactName;
static const char* _altitudeAMSLFactName;
static const char* _altitudeTuningFactName;
static const char* _altitudeTuningSetpointFactName;
static const char* _flightDistanceFactName;
static const char* _flightTimeFactName;
static const char* _distanceToHomeFactName;

Write Preview
Loading…
Cancel
Save