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

Add remaining orientations, drop multi-sensor

QGC4.4
DonLakeFlyer 7 years ago
parent
db490641d9
commit
0b8c03cb8e
3 changed files with 128 additions and 15 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. 42
      src/Vehicle/DistanceSensorFact.json
  2. 63
      src/Vehicle/Vehicle.cc
  3. 38
      src/Vehicle/Vehicle.h

42
src/Vehicle/DistanceSensorFact.json
Unescape View File

@ -7,6 +7,13 @@ @@ -7,6 +7,13 @@
"units": "m"
},
{
"name": "rotationYaw45",
"shortDescription": "Forward/Right",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw90",
"shortDescription": "Right",
"type": "double",
@ -14,6 +21,13 @@ @@ -14,6 +21,13 @@
"units": "m"
},
{
"name": "rotationYaw135",
"shortDescription": "Rear/Right",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw180",
"shortDescription": "Rear",
"type": "double",
@ -21,10 +35,38 @@ @@ -21,10 +35,38 @@
"units": "m"
},
{
"name": "rotationYaw225",
"shortDescription": "Rear/Left",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw270",
"shortDescription": "Left",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw315",
"shortDescription": "Forward/Left",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationPitch90",
"shortDescription": "Up",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationPitch270",
"shortDescription": "Down",
"type": "double",
"decimalPlaces": 2,
"units": "m"
}
]

63
src/Vehicle/Vehicle.cc
Unescape View File

@ -790,7 +790,17 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message) @@ -790,7 +790,17 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
{
mavlink_distance_sensor_t distanceSensor;
mavlink_msg_distance_sensor_decode(&message, &distanceSensor);
mavlink_msg_distance_sensor_decode(&message, &distanceSensor);\
if (!_distanceSensorFactGroup.idSet()) {
_distanceSensorFactGroup.setIdSet(true);
_id = distanceSensor.id;
}
if (_id != distanceSensor.id) {
// We can only handle a single sensor reporting
return;
}
struct orientation2Fact_s {
MAV_SENSOR_ORIENTATION orientation;
@ -799,16 +809,22 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message) @@ -799,16 +809,22 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
orientation2Fact_s rgOrientation2Fact[] =
{
{ MAV_SENSOR_ROTATION_NONE, _distanceSensorFactGroup.rotationNone() },
{ MAV_SENSOR_ROTATION_YAW_90, _distanceSensorFactGroup.rotationYaw90() },
{ MAV_SENSOR_ROTATION_YAW_180, _distanceSensorFactGroup.rotationYaw180() },
{ MAV_SENSOR_ROTATION_YAW_270, _distanceSensorFactGroup.rotationYaw270() },
{ MAV_SENSOR_ROTATION_NONE, _distanceSensorFactGroup.rotationNone() },
{ MAV_SENSOR_ROTATION_YAW_45, _distanceSensorFactGroup.rotationYaw45() },
{ MAV_SENSOR_ROTATION_YAW_90, _distanceSensorFactGroup.rotationYaw90() },
{ MAV_SENSOR_ROTATION_YAW_135, _distanceSensorFactGroup.rotationYaw135() },
{ MAV_SENSOR_ROTATION_YAW_180, _distanceSensorFactGroup.rotationYaw180() },
{ MAV_SENSOR_ROTATION_YAW_225, _distanceSensorFactGroup.rotationYaw225() },
{ MAV_SENSOR_ROTATION_YAW_270, _distanceSensorFactGroup.rotationYaw270() },
{ MAV_SENSOR_ROTATION_YAW_315, _distanceSensorFactGroup.rotationYaw315() },
{ MAV_SENSOR_ROTATION_PITCH_90, _distanceSensorFactGroup.rotationPitch90() },
{ MAV_SENSOR_ROTATION_PITCH_270, _distanceSensorFactGroup.rotationPitch270() },
};
for (size_t i=0; i<sizeof(rgOrientation2Fact)/sizeof(rgOrientation2Fact[0]); i++) {
const orientation2Fact_s& orientation2Fact = rgOrientation2Fact[i];
if (orientation2Fact.orientation == distanceSensor.orientation) {
orientation2Fact.fact->setRawValue(distanceSensor.current_distance);
orientation2Fact.fact->setRawValue(distanceSensor.current_distance / 100.0); // cm to meters
}
}
}
@ -3393,25 +3409,50 @@ VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent) @@ -3393,25 +3409,50 @@ VehicleSetpointFactGroup::VehicleSetpointFactGroup(QObject* parent)
}
const char* VehicleDistanceSensorFactGroup::_rotationNoneFactName = "rotationNone";
const char* VehicleDistanceSensorFactGroup::_rotationYaw45FactName = "rotationYaw45";
const char* VehicleDistanceSensorFactGroup::_rotationYaw90FactName = "rotationYaw90";
const char* VehicleDistanceSensorFactGroup::_rotationYaw135FactName = "rotationYaw135";
const char* VehicleDistanceSensorFactGroup::_rotationYaw180FactName = "rotationYaw180";
const char* VehicleDistanceSensorFactGroup::_rotationYaw225FactName = "rotationYaw225";
const char* VehicleDistanceSensorFactGroup::_rotationYaw270FactName = "rotationYaw270";
const char* VehicleDistanceSensorFactGroup::_rotationYaw315FactName = "rotationYaw315";
const char* VehicleDistanceSensorFactGroup::_rotationPitch90FactName = "rotationPitch90";
const char* VehicleDistanceSensorFactGroup::_rotationPitch270FactName = "rotationPitch270";
VehicleDistanceSensorFactGroup::VehicleDistanceSensorFactGroup(QObject* parent)
: FactGroup (1000, ":/json/Vehicle/DistanceSensorFact.json", parent)
, _rotationNoneFact (0, _rotationNoneFactName, FactMetaData::valueTypeDouble)
, _rotationYaw45Fact (0, _rotationYaw45FactName, FactMetaData::valueTypeDouble)
, _rotationYaw90Fact (0, _rotationYaw90FactName, FactMetaData::valueTypeDouble)
, _rotationYaw135Fact (0, _rotationYaw135FactName, FactMetaData::valueTypeDouble)
, _rotationYaw180Fact (0, _rotationYaw180FactName, FactMetaData::valueTypeDouble)
, _rotationYaw225Fact (0, _rotationYaw225FactName, FactMetaData::valueTypeDouble)
, _rotationYaw270Fact (0, _rotationYaw270FactName, FactMetaData::valueTypeDouble)
{
_addFact(&_rotationNoneFact, _rotationNoneFactName);
_addFact(&_rotationYaw90Fact, _rotationYaw90FactName);
_addFact(&_rotationYaw180Fact, _rotationYaw180FactName);
_addFact(&_rotationYaw270Fact, _rotationYaw270FactName);
, _rotationYaw315Fact (0, _rotationYaw315FactName, FactMetaData::valueTypeDouble)
, _rotationPitch90Fact (0, _rotationPitch90FactName, FactMetaData::valueTypeDouble)
, _rotationPitch270Fact (0, _rotationPitch270FactName, FactMetaData::valueTypeDouble)
, _idSet (false)
, _id (0)
{
_addFact(&_rotationNoneFact, _rotationNoneFactName);
_addFact(&_rotationYaw45Fact, _rotationYaw45FactName);
_addFact(&_rotationYaw90Fact, _rotationYaw90FactName);
_addFact(&_rotationYaw135Fact, _rotationYaw135FactName);
_addFact(&_rotationYaw180Fact, _rotationYaw180FactName);
_addFact(&_rotationYaw225Fact, _rotationYaw225FactName);
_addFact(&_rotationYaw270Fact, _rotationYaw270FactName);
_addFact(&_rotationYaw315Fact, _rotationYaw315FactName);
_addFact(&_rotationPitch90Fact, _rotationPitch90FactName);
_addFact(&_rotationPitch270Fact, _rotationPitch270FactName);
// Start out as not available "--.--"
_rotationNoneFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rotationYaw45Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rotationYaw135Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rotationYaw90Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rotationYaw180Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rotationYaw225Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rotationYaw270Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rotationPitch90Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_rotationPitch270Fact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}

38
src/Vehicle/Vehicle.h
Unescape View File

@ -47,26 +47,56 @@ class VehicleDistanceSensorFactGroup : public FactGroup @@ -47,26 +47,56 @@ class VehicleDistanceSensorFactGroup : public FactGroup
public:
VehicleDistanceSensorFactGroup(QObject* parent = NULL);
Q_PROPERTY(Fact* rotationNone READ rotationNone CONSTANT)
Q_PROPERTY(Fact* rotationYaw90 READ rotationYaw90 CONSTANT)
Q_PROPERTY(Fact* rotationYaw180 READ rotationYaw180 CONSTANT)
Q_PROPERTY(Fact* rotationYaw270 READ rotationYaw270 CONSTANT)
Q_PROPERTY(Fact* rotationNone READ rotationNone CONSTANT)
Q_PROPERTY(Fact* rotationYaw45 READ rotationYaw45 CONSTANT)
Q_PROPERTY(Fact* rotationYaw90 READ rotationYaw90 CONSTANT)
Q_PROPERTY(Fact* rotationYaw135 READ rotationYaw135 CONSTANT)
Q_PROPERTY(Fact* rotationYaw180 READ rotationYaw180 CONSTANT)
Q_PROPERTY(Fact* rotationYaw225 READ rotationYaw225 CONSTANT)
Q_PROPERTY(Fact* rotationYaw270 READ rotationYaw270 CONSTANT)
Q_PROPERTY(Fact* rotationYaw315 READ rotationYaw315 CONSTANT)
Q_PROPERTY(Fact* rotationPitch90 READ rotationPitch90 CONSTANT)
Q_PROPERTY(Fact* rotationPitch270 READ rotationPitch270 CONSTANT)
Fact* rotationNone (void) { return &_rotationNoneFact; }
Fact* rotationYaw45 (void) { return &_rotationYaw45Fact; }
Fact* rotationYaw90 (void) { return &_rotationYaw90Fact; }
Fact* rotationYaw135 (void) { return &_rotationYaw90Fact; }
Fact* rotationYaw180 (void) { return &_rotationYaw180Fact; }
Fact* rotationYaw225 (void) { return &_rotationYaw180Fact; }
Fact* rotationYaw270 (void) { return &_rotationYaw270Fact; }
Fact* rotationYaw315 (void) { return &_rotationYaw315Fact; }
Fact* rotationPitch90 (void) { return &_rotationPitch90Fact; }
Fact* rotationPitch270 (void) { return &_rotationPitch270Fact; }
bool idSet(void) { return _idSet; }
void setIdSet(bool idSet) { _idSet = idSet; }
static const char* _rotationNoneFactName;
static const char* _rotationYaw45FactName;
static const char* _rotationYaw90FactName;
static const char* _rotationYaw135FactName;
static const char* _rotationYaw180FactName;
static const char* _rotationYaw225FactName;
static const char* _rotationYaw270FactName;
static const char* _rotationYaw315FactName;
static const char* _rotationPitch90FactName;
static const char* _rotationPitch270FactName;
private:
Fact _rotationNoneFact;
Fact _rotationYaw45Fact;
Fact _rotationYaw90Fact;
Fact _rotationYaw135Fact;
Fact _rotationYaw180Fact;
Fact _rotationYaw225Fact;
Fact _rotationYaw270Fact;
Fact _rotationYaw315Fact;
Fact _rotationPitch90Fact;
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

Write Preview
Loading…
Cancel
Save