Newer
Older
qgcApp()->showMessage(QStringLiteral("Pause not supported by vehicle."));
return;
}
_firmwarePlugin->pauseVehicle(this);
}
Danny Schrader
committed
void Vehicle::abortLanding(double climbOutAltitude)
{
sendMavCommand(defaultComponentId(),
MAV_CMD_DO_GO_AROUND,
true, // show error if fails
Danny Schrader
committed
climbOutAltitude);
}
bool Vehicle::guidedMode(void) const
{
return _firmwarePlugin->isGuidedMode(this);
}
void Vehicle::setGuidedMode(bool guidedMode)
{
return _firmwarePlugin->setGuidedMode(this, guidedMode);
}
void Vehicle::emergencyStop(void)
{
MAV_CMD_COMPONENT_ARM_DISARM,
true, // show error if fails
0.0f,
21196.0f); // Magic number for emergency stop
void Vehicle::setCurrentMissionSequence(int seq)
{
if (!_firmwarePlugin->sendHomePositionToVehicle()) {
seq--;
}
mavlink_message_t msg;
mavlink_msg_mission_set_current_pack_chan(_mavlink->getSystemId(),
_mavlink->getComponentId(),
priorityLink()->mavlinkChannel(),
&msg,
id(),
_compID,
seq);
sendMessageOnLink(priorityLink(), msg);
void Vehicle::sendMavCommand(int component, MAV_CMD command, bool showError, float param1, float param2, float param3, float param4, float param5, float param6, float param7)
2052
2053
2054
2055
2056
2057
2058
2059
2060
2061
2062
2063
2064
2065
2066
2067
2068
2069
2070
2071
2072
2073
2074
MavCommandQueueEntry_t entry;
entry.component = component;
entry.command = command;
entry.showError = showError;
entry.rgParam[0] = param1;
entry.rgParam[1] = param2;
entry.rgParam[2] = param3;
entry.rgParam[3] = param4;
entry.rgParam[4] = param5;
entry.rgParam[5] = param6;
entry.rgParam[6] = param7;
_mavCommandQueue.append(entry);
if (_mavCommandQueue.count() == 1) {
_mavCommandRetryCount = 0;
_sendMavCommandAgain();
}
}
void Vehicle::_sendMavCommandAgain(void)
{
Gus Grubba
committed
if(!_mavCommandQueue.size()) {
qWarning() << "Command resend with no commands in queue";
_mavCommandAckTimer.stop();
return;
}
MavCommandQueueEntry_t& queuedCommand = _mavCommandQueue[0];
if (_mavCommandRetryCount++ > _mavCommandMaxRetryCount) {
if (queuedCommand.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
// We aren't going to get a response back for capabilities, so stop waiting for it before we ask for mission items
emit mavCommandResult(_id, queuedCommand.component, queuedCommand.command, MAV_RESULT_FAILED, true /* noResponsefromVehicle */);
if (queuedCommand.showError) {
qgcApp()->showMessage(tr("Vehicle did not respond to command: %1").arg(qgcApp()->toolbox()->missionCommandTree()->friendlyName(queuedCommand.command)));
}
_mavCommandQueue.removeFirst();
_sendNextQueuedMavCommand();
return;
}
// We always let AUTOPILOT_CAPABILITIES go through multiple times even if we don't get acks. This is because
// we really need to get capabilities and version info back over a lossy link.
if (queuedCommand.command != MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
if (px4Firmware()) {
// Older PX4 firmwares are inconsistent with repect to sending back an Ack from a COMMAND_LONG, hence we can't support retry logic for it.
if (_firmwareMajorVersion != versionNotSetValue) {
// If no version set assume lastest master dev build, so acks are suppored
if (_firmwareMajorVersion <= 1 && _firmwareMinorVersion <= 5 && _firmwarePatchVersion <= 3) {
// Acks not supported in this version
return;
}
}
} else {
if (queuedCommand.command == MAV_CMD_START_RX_PAIR) {
// The implementation of this command comes from the IO layer and is shared across stacks. So for other firmwares
// we aren't really sure whether they are correct or not.
return;
}
}
qCDebug(VehicleLog) << "Vehicle::_sendMavCommandAgain retrying command:_mavCommandRetryCount" << queuedCommand.command << _mavCommandRetryCount;
_mavCommandAckTimer.start();
mavlink_message_t msg;
mavlink_command_long_t cmd;
cmd.command = queuedCommand.command;
cmd.param1 = queuedCommand.rgParam[0];
cmd.param2 = queuedCommand.rgParam[1];
cmd.param3 = queuedCommand.rgParam[2];
cmd.param4 = queuedCommand.rgParam[3];
cmd.param5 = queuedCommand.rgParam[4];
cmd.param6 = queuedCommand.rgParam[5];
cmd.param7 = queuedCommand.rgParam[6];
cmd.target_system = _id;
cmd.target_component = queuedCommand.component;
mavlink_msg_command_long_encode_chan(_mavlink->getSystemId(),
_mavlink->getComponentId(),
priorityLink()->mavlinkChannel(),
&msg,
&cmd);
sendMessageOnLink(priorityLink(), msg);
void Vehicle::_sendNextQueuedMavCommand(void)
{
if (_mavCommandQueue.count()) {
_mavCommandRetryCount = 0;
_sendMavCommandAgain();
}
}
void Vehicle::setPrearmError(const QString& prearmError)
{
_prearmError = prearmError;
emit prearmErrorChanged(_prearmError);
if (!_prearmError.isEmpty()) {
_prearmErrorTimer.start();
}
}
void Vehicle::_prearmErrorTimeout(void)
{
setPrearmError(QString());
}
void Vehicle::setFirmwareVersion(int majorVersion, int minorVersion, int patchVersion, FIRMWARE_VERSION_TYPE versionType)
{
_firmwareMajorVersion = majorVersion;
_firmwareMinorVersion = minorVersion;
_firmwarePatchVersion = patchVersion;
2177
2178
2179
2180
2181
2182
2183
2184
2185
2186
2187
2188
2189
2190
2191
2192
2193
2194
2195
2196
2197
2198
_firmwareVersionType = versionType;
emit firmwareMajorVersionChanged(_firmwareMajorVersion);
emit firmwareMinorVersionChanged(_firmwareMinorVersion);
emit firmwarePatchVersionChanged(_firmwarePatchVersion);
emit firmwareVersionTypeChanged(_firmwareVersionType);
}
QString Vehicle::firmwareVersionTypeString(void) const
{
switch (_firmwareVersionType) {
case FIRMWARE_VERSION_TYPE_DEV:
return QStringLiteral("dev");
case FIRMWARE_VERSION_TYPE_ALPHA:
return QStringLiteral("alpha");
case FIRMWARE_VERSION_TYPE_BETA:
return QStringLiteral("beta");
case FIRMWARE_VERSION_TYPE_RC:
return QStringLiteral("rc");
case FIRMWARE_VERSION_TYPE_OFFICIAL:
default:
return QStringLiteral("");
}
sendMavCommand(_defaultComponentId, MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, true, 1.0f);
void Vehicle::setSoloFirmware(bool soloFirmware)
{
if (soloFirmware != _soloFirmware) {
_soloFirmware = soloFirmware;
emit soloFirmwareChanged(soloFirmware);
}
}
#if 0
// Temporarily removed, waiting for new command implementation
void Vehicle::motorTest(int motor, int percent, int timeoutSecs)
{
doCommandLongUnverified(_defaultComponentId, MAV_CMD_DO_MOTOR_TEST, motor, MOTOR_TEST_THROTTLE_PERCENT, percent, timeoutSecs);
QString Vehicle::brandImageIndoor(void) const
return _firmwarePlugin->brandImageIndoor(this);
}
QString Vehicle::brandImageOutdoor(void) const
{
return _firmwarePlugin->brandImageOutdoor(this);
2232
2233
2234
2235
2236
2237
2238
2239
2240
2241
2242
2243
2244
2245
2246
2247
2248
2249
2250
2251
2252
2253
2254
2255
2256
2257
2258
2259
2260
2261
2262
2263
2264
2265
2266
2267
2268
2269
2270
2271
2272
2273
2274
2275
2276
2277
2278
QStringList Vehicle::unhealthySensors(void) const
{
QStringList sensorList;
struct sensorInfo_s {
uint32_t bit;
const char* sensorName;
};
static const sensorInfo_s rgSensorInfo[] = {
{ MAV_SYS_STATUS_SENSOR_3D_GYRO, "Gyro" },
{ MAV_SYS_STATUS_SENSOR_3D_ACCEL, "Accelerometer" },
{ MAV_SYS_STATUS_SENSOR_3D_MAG, "Magnetometer" },
{ MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE, "Absolute pressure" },
{ MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE, "Differential pressure" },
{ MAV_SYS_STATUS_SENSOR_GPS, "GPS" },
{ MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW, "Optical flow" },
{ MAV_SYS_STATUS_SENSOR_VISION_POSITION, "Computer vision position" },
{ MAV_SYS_STATUS_SENSOR_LASER_POSITION, "Laser based position" },
{ MAV_SYS_STATUS_SENSOR_EXTERNAL_GROUND_TRUTH, "External ground truth" },
{ MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL, "Angular rate control" },
{ MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION, "Attitude stabilization" },
{ MAV_SYS_STATUS_SENSOR_YAW_POSITION, "Yaw position" },
{ MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL, "Z/altitude control" },
{ MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL, "X/Y position control" },
{ MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS, "Motor outputs / control" },
{ MAV_SYS_STATUS_SENSOR_RC_RECEIVER, "RC receiver" },
{ MAV_SYS_STATUS_SENSOR_3D_GYRO2, "Gyro 2" },
{ MAV_SYS_STATUS_SENSOR_3D_ACCEL2, "Accelerometer 2" },
{ MAV_SYS_STATUS_SENSOR_3D_MAG2, "Magnetometer 2" },
{ MAV_SYS_STATUS_GEOFENCE, "GeoFence" },
{ MAV_SYS_STATUS_AHRS, "AHRS" },
{ MAV_SYS_STATUS_TERRAIN, "Terrain" },
{ MAV_SYS_STATUS_REVERSE_MOTOR, "Motors reversed" },
{ MAV_SYS_STATUS_LOGGING, "Logging" },
};
for (size_t i=0; i<sizeof(rgSensorInfo)/sizeof(sensorInfo_s); i++) {
const sensorInfo_s* pSensorInfo = &rgSensorInfo[i];
if ((_onboardControlSensorsEnabled & pSensorInfo->bit) && !(_onboardControlSensorsHealth & pSensorInfo->bit)) {
sensorList << pSensorInfo->sensorName;
}
}
return sensorList;
}
void Vehicle::setOfflineEditingDefaultComponentId(int defaultComponentId)
{
if (_offlineEditingVehicle) {
_defaultComponentId = defaultComponentId;
} else {
qWarning() << "Call to Vehicle::setOfflineEditingDefaultComponentId on vehicle which is not offline";
}
}
MAV_CMD_DO_DIGICAM_CONTROL,
true, // show errors
0.0, 0.0, 0.0, 0.0, // param 1-4 unused
1.0); // trigger camera
}
const char* VehicleGPSFactGroup::_hdopFactName = "hdop";
const char* VehicleGPSFactGroup::_vdopFactName = "vdop";
const char* VehicleGPSFactGroup::_courseOverGroundFactName = "courseOverGround";
const char* VehicleGPSFactGroup::_countFactName = "count";
const char* VehicleGPSFactGroup::_lockFactName = "lock";
VehicleGPSFactGroup::VehicleGPSFactGroup(QObject* parent)
: FactGroup(1000, ":/json/Vehicle/GPSFact.json", parent)
, _hdopFact (0, _hdopFactName, FactMetaData::valueTypeDouble)
, _vdopFact (0, _vdopFactName, FactMetaData::valueTypeDouble)
, _courseOverGroundFact (0, _courseOverGroundFactName, FactMetaData::valueTypeDouble)
, _countFact (0, _countFactName, FactMetaData::valueTypeInt32)
, _lockFact (0, _lockFactName, FactMetaData::valueTypeInt32)
{
_addFact(&_hdopFact, _hdopFactName);
_addFact(&_vdopFact, _vdopFactName);
_addFact(&_courseOverGroundFact, _courseOverGroundFactName);
_addFact(&_lockFact, _lockFactName);
_addFact(&_countFact, _countFactName);
_hdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_vdopFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_courseOverGroundFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
void Vehicle::startMavlinkLog()
sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_START, false /* showError */);
sendMavCommand(_defaultComponentId, MAV_CMD_LOGGING_STOP, false /* showError */);
void Vehicle::_ackMavlinkLogData(uint16_t sequence)
{
mavlink_message_t msg;
mavlink_logging_ack_t ack;
ack.sequence = sequence;
ack.target_component = _defaultComponentId;
ack.target_system = id();
mavlink_msg_logging_ack_encode_chan(
_mavlink->getSystemId(),
_mavlink->getComponentId(),
priorityLink()->mavlinkChannel(),
&msg,
&ack);
sendMessageOnLink(priorityLink(), msg);
}
void Vehicle::_handleMavlinkLoggingData(mavlink_message_t& message)
{
mavlink_logging_data_t log;
mavlink_msg_logging_data_decode(&message, &log);
emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
log.first_message_offset, QByteArray((const char*)log.data, log.length), false);
void Vehicle::_handleMavlinkLoggingDataAcked(mavlink_message_t& message)
Beat Küng
committed
mavlink_logging_data_acked_t log;
mavlink_msg_logging_data_acked_decode(&message, &log);
_ackMavlinkLogData(log.sequence);
emit mavlinkLogData(this, log.target_system, log.target_component, log.sequence,
log.first_message_offset, QByteArray((const char*)log.data, log.length), true);
void Vehicle::setFirmwarePluginInstanceData(QObject* firmwarePluginInstanceData)
{
firmwarePluginInstanceData->setParent(this);
_firmwarePluginInstanceData = firmwarePluginInstanceData;
}
QString Vehicle::missionFlightMode(void) const
{
return _firmwarePlugin->missionFlightMode();
}
QString Vehicle::pauseFlightMode(void) const
{
return _firmwarePlugin->pauseFlightMode();
}
QString Vehicle::rtlFlightMode(void) const
{
return _firmwarePlugin->rtlFlightMode();
}
QString Vehicle::landFlightMode(void) const
{
return _firmwarePlugin->landFlightMode();
}
QString Vehicle::takeControlFlightMode(void) const
{
return _firmwarePlugin->takeControlFlightMode();
}
2397
2398
2399
2400
2401
2402
2403
2404
2405
2406
2407
2408
2409
2410
2411
2412
2413
2414
2415
2416
2417
2418
2419
2420
QString Vehicle::vehicleImageOpaque() const
{
if(_firmwarePlugin)
return _firmwarePlugin->vehicleImageOpaque(this);
else
return QString();
}
QString Vehicle::vehicleImageOutline() const
{
if(_firmwarePlugin)
return _firmwarePlugin->vehicleImageOutline(this);
else
return QString();
}
QString Vehicle::vehicleImageCompass() const
{
if(_firmwarePlugin)
return _firmwarePlugin->vehicleImageCompass(this);
else
return QString();
}
const QVariantList& Vehicle::toolBarIndicators()
{
if(_firmwarePlugin) {
return _firmwarePlugin->toolBarIndicators(this);
}
static QVariantList emptyList;
return emptyList;
}
const QVariantList& Vehicle::cameraList(void) const
{
if (_firmwarePlugin) {
return _firmwarePlugin->cameraList(this);
}
static QVariantList emptyList;
return emptyList;
}
bool Vehicle::vehicleYawsToNextWaypointInMission(void) const
{
return _firmwarePlugin->vehicleYawsToNextWaypointInMission(this);
}
2444
2445
2446
2447
2448
2449
2450
2451
2452
2453
2454
2455
2456
2457
2458
2459
2460
2461
2462
2463
2464
2465
2466
void Vehicle::_setupAutoDisarmSignalling(void)
{
QString param = _firmwarePlugin->autoDisarmParameter(this);
if (!param.isEmpty() && _parameterManager->parameterExists(FactSystem::defaultComponentId, param)) {
Fact* fact = _parameterManager->getParameter(FactSystem::defaultComponentId,param);
connect(fact, &Fact::rawValueChanged, this, &Vehicle::autoDisarmChanged);
emit autoDisarmChanged();
}
}
bool Vehicle::autoDisarm(void)
{
QString param = _firmwarePlugin->autoDisarmParameter(this);
if (!param.isEmpty() && _parameterManager->parameterExists(FactSystem::defaultComponentId, param)) {
Fact* fact = _parameterManager->getParameter(FactSystem::defaultComponentId,param);
return fact->rawValue().toDouble() > 0;
}
return false;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
const char* VehicleBatteryFactGroup::_voltageFactName = "voltage";
const char* VehicleBatteryFactGroup::_percentRemainingFactName = "percentRemaining";
const char* VehicleBatteryFactGroup::_mahConsumedFactName = "mahConsumed";
const char* VehicleBatteryFactGroup::_currentFactName = "current";
const char* VehicleBatteryFactGroup::_temperatureFactName = "temperature";
const char* VehicleBatteryFactGroup::_cellCountFactName = "cellCount";
const char* VehicleBatteryFactGroup::_settingsGroup = "Vehicle.battery";
const double VehicleBatteryFactGroup::_voltageUnavailable = -1.0;
const int VehicleBatteryFactGroup::_percentRemainingUnavailable = -1;
const int VehicleBatteryFactGroup::_mahConsumedUnavailable = -1;
const int VehicleBatteryFactGroup::_currentUnavailable = -1;
const double VehicleBatteryFactGroup::_temperatureUnavailable = -1.0;
const int VehicleBatteryFactGroup::_cellCountUnavailable = -1.0;
VehicleBatteryFactGroup::VehicleBatteryFactGroup(QObject* parent)
: FactGroup(1000, ":/json/Vehicle/BatteryFact.json", parent)
, _voltageFact (0, _voltageFactName, FactMetaData::valueTypeDouble)
, _percentRemainingFact (0, _percentRemainingFactName, FactMetaData::valueTypeInt32)
, _mahConsumedFact (0, _mahConsumedFactName, FactMetaData::valueTypeInt32)
, _currentFact (0, _currentFactName, FactMetaData::valueTypeFloat)
, _temperatureFact (0, _temperatureFactName, FactMetaData::valueTypeDouble)
, _cellCountFact (0, _cellCountFactName, FactMetaData::valueTypeInt32)
{
_addFact(&_voltageFact, _voltageFactName);
_addFact(&_percentRemainingFact, _percentRemainingFactName);
_addFact(&_mahConsumedFact, _mahConsumedFactName);
_addFact(&_currentFact, _currentFactName);
_addFact(&_temperatureFact, _temperatureFactName);
_addFact(&_cellCountFact, _cellCountFactName);
// Start out as not available
_voltageFact.setRawValue (_voltageUnavailable);
_percentRemainingFact.setRawValue (_percentRemainingUnavailable);
_mahConsumedFact.setRawValue (_mahConsumedUnavailable);
_currentFact.setRawValue (_currentUnavailable);
_temperatureFact.setRawValue (_temperatureUnavailable);
_cellCountFact.setRawValue (_cellCountUnavailable);
}
const char* VehicleWindFactGroup::_directionFactName = "direction";
const char* VehicleWindFactGroup::_speedFactName = "speed";
const char* VehicleWindFactGroup::_verticalSpeedFactName = "verticalSpeed";
VehicleWindFactGroup::VehicleWindFactGroup(QObject* parent)
: FactGroup(1000, ":/json/Vehicle/WindFact.json", parent)
, _directionFact (0, _directionFactName, FactMetaData::valueTypeDouble)
, _speedFact (0, _speedFactName, FactMetaData::valueTypeDouble)
, _verticalSpeedFact(0, _verticalSpeedFactName, FactMetaData::valueTypeDouble)
{
_addFact(&_directionFact, _directionFactName);
_addFact(&_speedFact, _speedFactName);
_addFact(&_verticalSpeedFact, _verticalSpeedFactName);
// Start out as not available "--.--"
_directionFact.setRawValue (std::numeric_limits<float>::quiet_NaN());
_speedFact.setRawValue (std::numeric_limits<float>::quiet_NaN());
_verticalSpeedFact.setRawValue (std::numeric_limits<float>::quiet_NaN());
}
const char* VehicleVibrationFactGroup::_xAxisFactName = "xAxis";
const char* VehicleVibrationFactGroup::_yAxisFactName = "yAxis";
const char* VehicleVibrationFactGroup::_zAxisFactName = "zAxis";
const char* VehicleVibrationFactGroup::_clipCount1FactName = "clipCount1";
const char* VehicleVibrationFactGroup::_clipCount2FactName = "clipCount2";
const char* VehicleVibrationFactGroup::_clipCount3FactName = "clipCount3";
2538
2539
2540
2541
2542
2543
2544
2545
2546
2547
2548
2549
2550
2551
2552
2553
2554
2555
2556
2557
2558
VehicleVibrationFactGroup::VehicleVibrationFactGroup(QObject* parent)
: FactGroup(1000, ":/json/Vehicle/VibrationFact.json", parent)
, _xAxisFact (0, _xAxisFactName, FactMetaData::valueTypeDouble)
, _yAxisFact (0, _yAxisFactName, FactMetaData::valueTypeDouble)
, _zAxisFact (0, _zAxisFactName, FactMetaData::valueTypeDouble)
, _clipCount1Fact (0, _clipCount1FactName, FactMetaData::valueTypeUint32)
, _clipCount2Fact (0, _clipCount2FactName, FactMetaData::valueTypeUint32)
, _clipCount3Fact (0, _clipCount3FactName, FactMetaData::valueTypeUint32)
{
_addFact(&_xAxisFact, _xAxisFactName);
_addFact(&_yAxisFact, _yAxisFactName);
_addFact(&_zAxisFact, _zAxisFactName);
_addFact(&_clipCount1Fact, _clipCount1FactName);
_addFact(&_clipCount2Fact, _clipCount2FactName);
_addFact(&_clipCount3Fact, _clipCount3FactName);
// Start out as not available "--.--"
_xAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_yAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
_zAxisFact.setRawValue(std::numeric_limits<float>::quiet_NaN());
}
2559
2560
2561
2562
2563
2564
2565
2566
2567
2568
2569
2570
2571
2572
2573
2574
2575
2576
2577
2578
2579
const char* VehicleTemperatureFactGroup::_temperature1FactName = "temperature1";
const char* VehicleTemperatureFactGroup::_temperature2FactName = "temperature2";
const char* VehicleTemperatureFactGroup::_temperature3FactName = "temperature3";
VehicleTemperatureFactGroup::VehicleTemperatureFactGroup(QObject* parent)
: FactGroup(1000, ":/json/Vehicle/TemperatureFact.json", parent)
, _temperature1Fact (0, _temperature1FactName, FactMetaData::valueTypeDouble)
, _temperature2Fact (0, _temperature2FactName, FactMetaData::valueTypeDouble)
, _temperature3Fact (0, _temperature3FactName, FactMetaData::valueTypeDouble)
{
_addFact(&_temperature1Fact, _temperature1FactName);
_addFact(&_temperature2Fact, _temperature2FactName);
_addFact(&_temperature3Fact, _temperature3FactName);
// Start out as not available "--.--"
_temperature1Fact.setRawValue (std::numeric_limits<float>::quiet_NaN());
_temperature2Fact.setRawValue (std::numeric_limits<float>::quiet_NaN());
_temperature3Fact.setRawValue (std::numeric_limits<float>::quiet_NaN());
}