Skip to content

Q
qgroundcontrol
Commit 0b8c03cb authored by DonLakeFlyer's avatar DonLakeFlyer
Browse files
Options

Add remaining orientations, drop multi-sensor

parent db490641
Show whitespace changes
Inline Side-by-side
Showing with 128 additions and 15 deletions
src/Vehicle/DistanceSensorFact.json
View file @ 0b8c03cb
......@@ -6,6 +6,13 @@
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw45",
"shortDescription": "Forward/Right",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw90",
"shortDescription": "Right",
......@@ -13,6 +20,13 @@
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw135",
"shortDescription": "Rear/Right",
"type": "double",
"decimalPlaces": 2,
"units": "m"
},
{
"name": "rotationYaw180",
"shortDescription": "Rear",
......@@ -20,11 +34,39 @@
"decimalPlaces": 2,
"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"
}
]
src/Vehicle/Vehicle.cc
View file @ 0b8c03cb
......@@ -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;
......@@ -800,15 +810,21 @@ void Vehicle::_handleDistanceSensor(mavlink_message_t& message)
orientation2Fact_s rgOrientation2Fact[] =
{
{ 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)
}
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)
, _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());
}
src/Vehicle/Vehicle.h
View file @ 0b8c03cb
......@@ -48,25 +48,55 @@ public:
VehicleDistanceSensorFactGroup(QObject* parent = NULL);
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
......
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