Browse Source

Fix distance sensor telemetry reading

QGC4.4
DonLakeFlyer 5 years ago
parent
commit
88be157fea
  1. 1
      QGCCommon.pri
  2. 14
      src/Vehicle/Vehicle.cc
  3. 8
      src/Vehicle/Vehicle.h

1
QGCCommon.pri

@ -181,7 +181,6 @@ installer {
# Setup our supported build flavors # Setup our supported build flavors
message($$CONFIG)
CONFIG(debug, debug|release) { CONFIG(debug, debug|release) {
message(Debug flavor) message(Debug flavor)
CONFIG += DebugBuild CONFIG += DebugBuild

14
src/Vehicle/Vehicle.cc

@ -1030,17 +1030,9 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
{ {
mavlink_distance_sensor_t distanceSensor; mavlink_distance_sensor_t distanceSensor;
mavlink_msg_distance_sensor_decode(&message, &distanceSensor);\ mavlink_msg_distance_sensor_decode(&message, &distanceSensor);
if (!_distanceSensorFactGroup.idSet()) { qDebug() << distanceSensor.id << distanceSensor.orientation << distanceSensor.current_distance;
_distanceSensorFactGroup.setIdSet(true);
_distanceSensorFactGroup.setId(distanceSensor.id);
}
if (_distanceSensorFactGroup.id() != distanceSensor.id) {
// We can only handle a single sensor reporting
return;
}
struct orientation2Fact_s { struct orientation2Fact_s {
MAV_SENSOR_ORIENTATION orientation; MAV_SENSOR_ORIENTATION orientation;
@ -4354,8 +4346,6 @@ VehicleDistanceSensorFactGroup::VehicleDistanceSensorFactGroup(QObject* parent)
, _rotationYaw315Fact (0, _rotationYaw315FactName, FactMetaData::valueTypeDouble) , _rotationYaw315Fact (0, _rotationYaw315FactName, FactMetaData::valueTypeDouble)
, _rotationPitch90Fact (0, _rotationPitch90FactName, FactMetaData::valueTypeDouble) , _rotationPitch90Fact (0, _rotationPitch90FactName, FactMetaData::valueTypeDouble)
, _rotationPitch270Fact (0, _rotationPitch270FactName, FactMetaData::valueTypeDouble) , _rotationPitch270Fact (0, _rotationPitch270FactName, FactMetaData::valueTypeDouble)
, _idSet (false)
, _id (0)
{ {
_addFact(&_rotationNoneFact, _rotationNoneFactName); _addFact(&_rotationNoneFact, _rotationNoneFactName);
_addFact(&_rotationYaw45Fact, _rotationYaw45FactName); _addFact(&_rotationYaw45Fact, _rotationYaw45FactName);

8
src/Vehicle/Vehicle.h

@ -76,11 +76,6 @@ public:
Fact* rotationPitch90 (void) { return &_rotationPitch90Fact; } Fact* rotationPitch90 (void) { return &_rotationPitch90Fact; }
Fact* rotationPitch270 (void) { return &_rotationPitch270Fact; } Fact* rotationPitch270 (void) { return &_rotationPitch270Fact; }
bool idSet(void) { return _idSet; }
void setIdSet(bool idSet) { _idSet = idSet; }
uint8_t id(void) { return _id; }
void setId(uint8_t id) { _id = id; }
static const char* _rotationNoneFactName; static const char* _rotationNoneFactName;
static const char* _rotationYaw45FactName; static const char* _rotationYaw45FactName;
static const char* _rotationYaw90FactName; static const char* _rotationYaw90FactName;
@ -103,9 +98,6 @@ private:
Fact _rotationYaw315Fact; Fact _rotationYaw315Fact;
Fact _rotationPitch90Fact; Fact _rotationPitch90Fact;
Fact _rotationPitch270Fact; Fact _rotationPitch270Fact;
bool _idSet; // true: _id is set to seen sensor id
uint8_t _id; // The id for the sensor being tracked. Current support for only a single sensor.
}; };
class VehicleSetpointFactGroup : public FactGroup class VehicleSetpointFactGroup : public FactGroup

Loading…
Cancel
Save