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

Support mag/gyro/accel cal response

QGC4.4
Don Gagne 9 years ago
parent
92de1d2136
commit
8a1495f6c3
2 changed files with 121 additions and 87 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 207
      src/comm/MockLink.cc
  2. 1
      src/comm/MockLink.h

207
src/comm/MockLink.cc
Unescape View File

@ -28,13 +28,13 @@ QGC_LOGGING_CATEGORY(MockLinkVerboseLog, "MockLinkVerboseLog")
enum PX4_CUSTOM_MAIN_MODE { enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1, PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_ALTCTL, PX4_CUSTOM_MAIN_MODE_ALTCTL,
PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO, PX4_CUSTOM_MAIN_MODE_ACRO,
PX4_CUSTOM_MAIN_MODE_OFFBOARD, PX4_CUSTOM_MAIN_MODE_OFFBOARD,
PX4_CUSTOM_MAIN_MODE_STABILIZED, PX4_CUSTOM_MAIN_MODE_STABILIZED,
PX4_CUSTOM_MAIN_MODE_RATTITUDE PX4_CUSTOM_MAIN_MODE_RATTITUDE
}; };
enum PX4_CUSTOM_SUB_MODE_AUTO { enum PX4_CUSTOM_SUB_MODE_AUTO {
@ -226,31 +226,31 @@ void MockLink::_loadParams(void)
QVariant paramValue; QVariant paramValue;
switch (paramType) { switch (paramType) {
case MAV_PARAM_TYPE_REAL32: case MAV_PARAM_TYPE_REAL32:
paramValue = QVariant(valStr.toFloat()); paramValue = QVariant(valStr.toFloat());
break; break;
case MAV_PARAM_TYPE_UINT32: case MAV_PARAM_TYPE_UINT32:
paramValue = QVariant(valStr.toUInt()); paramValue = QVariant(valStr.toUInt());
break; break;
case MAV_PARAM_TYPE_INT32: case MAV_PARAM_TYPE_INT32:
paramValue = QVariant(valStr.toInt()); paramValue = QVariant(valStr.toInt());
break; break;
case MAV_PARAM_TYPE_UINT16: case MAV_PARAM_TYPE_UINT16:
paramValue = QVariant((quint16)valStr.toUInt()); paramValue = QVariant((quint16)valStr.toUInt());
break; break;
case MAV_PARAM_TYPE_INT16: case MAV_PARAM_TYPE_INT16:
paramValue = QVariant((qint16)valStr.toInt()); paramValue = QVariant((qint16)valStr.toInt());
break; break;
case MAV_PARAM_TYPE_UINT8: case MAV_PARAM_TYPE_UINT8:
paramValue = QVariant((quint8)valStr.toUInt()); paramValue = QVariant((quint8)valStr.toUInt());
break; break;
case MAV_PARAM_TYPE_INT8: case MAV_PARAM_TYPE_INT8:
paramValue = QVariant((qint8)valStr.toUInt()); paramValue = QVariant((qint8)valStr.toUInt());
break; break;
default: default:
qCritical() << "Unknown type" << paramType; qCritical() << "Unknown type" << paramType;
paramValue = QVariant(valStr.toInt()); paramValue = QVariant(valStr.toInt());
break; break;
} }
qCDebug(MockLinkVerboseLog) << "Loading param" << paramName << paramValue; qCDebug(MockLinkVerboseLog) << "Loading param" << paramName << paramValue;
@ -359,40 +359,40 @@ void MockLink::_handleIncomingMavlinkBytes(const uint8_t* bytes, int cBytes)
} }
switch (msg.msgid) { switch (msg.msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: case MAVLINK_MSG_ID_HEARTBEAT:
_handleHeartBeat(msg); _handleHeartBeat(msg);
break; break;
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: case MAVLINK_MSG_ID_PARAM_REQUEST_LIST:
_handleParamRequestList(msg); _handleParamRequestList(msg);
break; break;
case MAVLINK_MSG_ID_SET_MODE: case MAVLINK_MSG_ID_SET_MODE:
_handleSetMode(msg); _handleSetMode(msg);
break; break;
case MAVLINK_MSG_ID_PARAM_SET: case MAVLINK_MSG_ID_PARAM_SET:
_handleParamSet(msg); _handleParamSet(msg);
break; break;
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: case MAVLINK_MSG_ID_PARAM_REQUEST_READ:
_handleParamRequestRead(msg); _handleParamRequestRead(msg);
break; break;
case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL:
_handleFTP(msg); _handleFTP(msg);
break; break;
case MAVLINK_MSG_ID_COMMAND_LONG: case MAVLINK_MSG_ID_COMMAND_LONG:
_handleCommandLong(msg); _handleCommandLong(msg);
break; break;
case MAVLINK_MSG_ID_MANUAL_CONTROL: case MAVLINK_MSG_ID_MANUAL_CONTROL:
_handleManualControl(msg); _handleManualControl(msg);
break; break;
default: default:
break; break;
} }
} }
} }
@ -489,7 +489,7 @@ float MockLink::_floatUnionForParam(int componentId, const QString& paramName)
switch (paramType) { switch (paramType) {
case MAV_PARAM_TYPE_REAL32: case MAV_PARAM_TYPE_REAL32:
valueUnion.param_float = paramVar.toFloat(); valueUnion.param_float = paramVar.toFloat();
break; break;
case MAV_PARAM_TYPE_UINT32: case MAV_PARAM_TYPE_UINT32:
@ -747,24 +747,24 @@ void MockLink::emitRemoteControlChannelRawChanged(int channel, uint16_t raw)
0, // time since boot, ignored 0, // time since boot, ignored
18, // channel count 18, // channel count
chanRaw[0], // channel raw value chanRaw[0], // channel raw value
chanRaw[1], // channel raw value chanRaw[1], // channel raw value
chanRaw[2], // channel raw value chanRaw[2], // channel raw value
chanRaw[3], // channel raw value chanRaw[3], // channel raw value
chanRaw[4], // channel raw value chanRaw[4], // channel raw value
chanRaw[5], // channel raw value chanRaw[5], // channel raw value
chanRaw[6], // channel raw value chanRaw[6], // channel raw value
chanRaw[7], // channel raw value chanRaw[7], // channel raw value
chanRaw[8], // channel raw value chanRaw[8], // channel raw value
chanRaw[9], // channel raw value chanRaw[9], // channel raw value
chanRaw[10], // channel raw value chanRaw[10], // channel raw value
chanRaw[11], // channel raw value chanRaw[11], // channel raw value
chanRaw[12], // channel raw value chanRaw[12], // channel raw value
chanRaw[13], // channel raw value chanRaw[13], // channel raw value
chanRaw[14], // channel raw value chanRaw[14], // channel raw value
chanRaw[15], // channel raw value chanRaw[15], // channel raw value
chanRaw[16], // channel raw value chanRaw[16], // channel raw value
chanRaw[17], // channel raw value chanRaw[17], // channel raw value
0); // rss 0); // rss
respondWithMavlinkMessage(responseMsg); respondWithMavlinkMessage(responseMsg);
} }
@ -791,6 +791,9 @@ void MockLink::_handleCommandLong(const mavlink_message_t& msg)
commandResult = MAV_RESULT_ACCEPTED; commandResult = MAV_RESULT_ACCEPTED;
break; break;
case MAV_CMD_PREFLIGHT_CALIBRATION: case MAV_CMD_PREFLIGHT_CALIBRATION:
_handlePreFlightCalibration(request);
commandResult = MAV_RESULT_ACCEPTED;
break;
case MAV_CMD_PREFLIGHT_STORAGE: case MAV_CMD_PREFLIGHT_STORAGE:
commandResult = MAV_RESULT_ACCEPTED; commandResult = MAV_RESULT_ACCEPTED;
break; break;
@ -889,16 +892,16 @@ void MockLink::_sendStatusTextMessages(void)
}; };
static const struct StatusMessage rgMessages[] = { static const struct StatusMessage rgMessages[] = {
{ MAV_SEVERITY_INFO, "#Testing audio output" }, { MAV_SEVERITY_INFO, "#Testing audio output" },
{ MAV_SEVERITY_EMERGENCY, "Status text emergency" }, { MAV_SEVERITY_EMERGENCY, "Status text emergency" },
{ MAV_SEVERITY_ALERT, "Status text alert" }, { MAV_SEVERITY_ALERT, "Status text alert" },
{ MAV_SEVERITY_CRITICAL, "Status text critical" }, { MAV_SEVERITY_CRITICAL, "Status text critical" },
{ MAV_SEVERITY_ERROR, "Status text error" }, { MAV_SEVERITY_ERROR, "Status text error" },
{ MAV_SEVERITY_WARNING, "Status text warning" }, { MAV_SEVERITY_WARNING, "Status text warning" },
{ MAV_SEVERITY_NOTICE, "Status text notice" }, { MAV_SEVERITY_NOTICE, "Status text notice" },
{ MAV_SEVERITY_INFO, "Status text info" }, { MAV_SEVERITY_INFO, "Status text info" },
{ MAV_SEVERITY_DEBUG, "Status text debug" }, { MAV_SEVERITY_DEBUG, "Status text debug" },
}; };
for (size_t i=0; i<sizeof(rgMessages)/sizeof(rgMessages[0]); i++) { for (size_t i=0; i<sizeof(rgMessages)/sizeof(rgMessages[0]); i++) {
mavlink_message_t msg; mavlink_message_t msg;
@ -1053,3 +1056,33 @@ void MockLink::_sendRCChannels(void)
respondWithMavlinkMessage(msg); respondWithMavlinkMessage(msg);
} }
void MockLink::_handlePreFlightCalibration(const mavlink_command_long_t& request)
{
const char* pCalMessage;
static const char* gyroCalResponse = "[cal] calibration started: 2 gyro";
static const char* magCalResponse = "[cal] calibration started: 2 mag";
static const char* accelCalResponse = "[cal] calibration started: 2 accel";
if (request.param1 == 1) {
// Gyro cal
pCalMessage = gyroCalResponse;
} else if (request.param2 == 1) {
// Mag cal
pCalMessage = magCalResponse;
} else if (request.param5 == 1) {
// Accel cal
pCalMessage = accelCalResponse;
} else {
return;
}
mavlink_message_t msg;
mavlink_msg_statustext_pack(_vehicleSystemId,
_vehicleComponentId,
&msg,
MAV_SEVERITY_INFO,
pCalMessage);
respondWithMavlinkMessage(msg);
}

1
src/comm/MockLink.h
Unescape View File

@ -166,6 +166,7 @@ private:
void _handleFTP(const mavlink_message_t& msg); void _handleFTP(const mavlink_message_t& msg);
void _handleCommandLong(const mavlink_message_t& msg); void _handleCommandLong(const mavlink_message_t& msg);
void _handleManualControl(const mavlink_message_t& msg); void _handleManualControl(const mavlink_message_t& msg);
void _handlePreFlightCalibration(const mavlink_command_long_t& request);
float _floatUnionForParam(int componentId, const QString& paramName); float _floatUnionForParam(int componentId, const QString& paramName);
void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat); void _setParamFloatUnionIntoMap(int componentId, const QString& paramName, float paramFloat);
void _sendHomePosition(void); void _sendHomePosition(void);

Write Preview
Loading…
Cancel
Save