Skip to content

Q
qgroundcontrol
Commit 6235763f authored by Don Gagne's avatar Don Gagne
Browse files
Options

New start/stop calibration apis

parent 5b81d161
Hide whitespace changes
Inline Side-by-side
Showing with 77 additions and 51 deletions
src/AutoPilotPlugins/PX4/FlightModesComponentController.cc
View file @ 6235763f
......@@ -153,11 +153,11 @@ void FlightModesComponentController::setSendLiveRCSwitchRanges(bool start)
_rgRCReversed[i] = floatReversed == -1.0f;
}
_uas->startRadioControlCalibration();
_uas->startCalibration(UASInterface::StartCalibrationRadio);
connect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged);
} else {
disconnect(_uas, &UASInterface::remoteControlChannelRawChanged, this, &FlightModesComponentController::_remoteControlChannelRawChanged);
_uas->endRadioControlCalibration();
_uas->stopCalibration();
_initRcValues();
emit switchLiveRangeChanged();
}
......
src/qgcunittest/MockUAS.h
View file @ 6235763f
......@@ -105,6 +105,8 @@ public:
virtual QString getSystemTypeName() { Q_ASSERT(false); return _bogusString; };
virtual int getAutopilotType() { return MAV_AUTOPILOT_PX4; };
virtual QGCUASFileManager* getFileManager() {Q_ASSERT(false); return NULL; }
virtual void startCalibration(StartCalibrationType calType) { Q_UNUSED(calType); return; };
virtual void stopCalibration() { Q_ASSERT(false); return; };
/** @brief Send a message over this link (to this or to all UAS on this link) */
virtual void sendMessage(LinkInterface* link, mavlink_message_t message){ Q_UNUSED(link); Q_UNUSED(message); Q_ASSERT(false); }
......@@ -152,11 +154,6 @@ public slots:
virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw)
{ Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); Q_ASSERT(false); };
virtual void setLocalPositionOffset(float x, float y, float z, float yaw) { Q_UNUSED(x); Q_UNUSED(y); Q_UNUSED(z); Q_UNUSED(yaw); Q_ASSERT(false); };
virtual void startRadioControlCalibration(int param) { Q_UNUSED(param); return; };
virtual void endRadioControlCalibration() { return; };
virtual void startMagnetometerCalibration() { Q_ASSERT(false); };
virtual void startGyroscopeCalibration() { Q_ASSERT(false); };
virtual void startPressureCalibration() { Q_ASSERT(false); };
virtual void setBatterySpecs(const QString& specs) { Q_UNUSED(specs); Q_ASSERT(false); };
virtual QString getBatterySpecs() { Q_ASSERT(false); return _bogusString; };
virtual void sendHilState(quint64 time_us, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, double lat, double lon, double alt, float vx, float vy, float vz, float ind_airspeed, float true_airspeed, float xacc, float yacc, float zacc)
......
src/uas/UAS.cc
View file @ 6235763f
......@@ -1439,19 +1439,67 @@ void UAS::setLocalPositionOffset(float x, float y, float z, float yaw)
Q_UNUSED(yaw);
}
void UAS::startRadioControlCalibration(int param)
void UAS::startCalibration(UASInterface::StartCalibrationType calType)
{
int gyroCal = 0;
int magCal = 0;
int airspeedCal = 0;
int radioCal = 0;
int accelCal = 0;
switch (calType) {
case StartCalibrationGyro:
gyroCal = 1;
break;
case StartCalibrationMag:
magCal = 1;
break;
case StartCalibrationAirspeed:
airspeedCal = 1;
break;
case StartCalibrationRadio:
radioCal = 1;
break;
case StartCalibrationAccel:
accelCal = 1;
break;
}
mavlink_message_t msg;
// Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, param, 0, 0, 0);
mavlink_msg_command_long_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
&msg,
uasId,
0, // target component
MAV_CMD_PREFLIGHT_CALIBRATION, // command id
0, // 0=first transmission of command
gyroCal, // gyro cal
magCal, // mag cal
0, // ground pressure
radioCal, // radio cal
accelCal, // accel cal
airspeedCal, // airspeed cal
0); // unused
sendMessage(msg);
}
void UAS::endRadioControlCalibration()
void UAS::stopCalibration(void)
{
mavlink_message_t msg;
// Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, 0, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 0, 0, 0, 0, 0);
mavlink_msg_command_long_pack(mavlink->getSystemId(),
mavlink->getComponentId(),
&msg,
uasId,
0, // target component
MAV_CMD_PREFLIGHT_CALIBRATION, // command id
0, // 0=first transmission of command
0, // gyro cal
0, // mag cal
0, // ground pressure
0, // radio cal
0, // accel cal
0, // airspeed cal
0); // unused
sendMessage(msg);
}
......@@ -1469,30 +1517,6 @@ void UAS::stopDataRecording()
sendMessage(msg);
}
void UAS::startMagnetometerCalibration()
{
mavlink_message_t msg;
// Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 1, 0, 0, 0, 0, 0);
sendMessage(msg);
}
void UAS::startGyroscopeCalibration()
{
mavlink_message_t msg;
// Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 1, 0, 0, 0, 0, 0, 0);
sendMessage(msg);
}
void UAS::startPressureCalibration()
{
mavlink_message_t msg;
// Param 1: gyro cal, param 2: mag cal, param 3: pressure cal, Param 4: radio
mavlink_msg_command_long_pack(mavlink->getSystemId(), mavlink->getComponentId(), &msg, uasId, MAV_COMP_ID_IMU, MAV_CMD_PREFLIGHT_CALIBRATION, 1, 0, 0, 1, 0, 0, 0, 0);
sendMessage(msg);
}
/**
* Check if time is smaller than 40 years, assuming no system without Unix
* timestamp runs longer than 40 years continuously without reboot. In worst case
......
src/uas/UAS.h
View file @ 6235763f
......@@ -863,11 +863,8 @@ public slots:
/** @brief Add an offset in body frame to the setpoint */
void setLocalPositionOffset(float x, float y, float z, float yaw);
void startRadioControlCalibration(int param=1);
void endRadioControlCalibration();
void startMagnetometerCalibration();
void startGyroscopeCalibration();
void startPressureCalibration();
void startCalibration(StartCalibrationType calType);
void stopCalibration(void);
void startDataRecording();
void stopDataRecording();
......
src/uas/UASInterface.h
View file @ 6235763f
......@@ -242,6 +242,20 @@ public:
static const unsigned int WAYPOINT_RADIUS_DEFAULT_FIXED_WING = 25;
static const unsigned int WAYPOINT_RADIUS_DEFAULT_ROTARY_WING = 5;
enum StartCalibrationType {
StartCalibrationRadio,
StartCalibrationGyro,
StartCalibrationMag,
StartCalibrationAirspeed,
StartCalibrationAccel
};
/// Starts the specified calibration
virtual void startCalibration(StartCalibrationType calType) = 0;
/// Ends any current calibration
virtual void stopCalibration(void) = 0;
public slots:
......@@ -344,12 +358,6 @@ public slots:
virtual void setLocalPositionSetpoint(float x, float y, float z, float yaw) = 0;
virtual void setLocalPositionOffset(float x, float y, float z, float yaw) = 0;
virtual void startRadioControlCalibration(int param=1) = 0;
virtual void endRadioControlCalibration() = 0;
virtual void startMagnetometerCalibration() = 0;
virtual void startGyroscopeCalibration() = 0;
virtual void startPressureCalibration() = 0;
/** @brief Return if this a rotary wing */
virtual bool isRotaryWing() = 0;
/** @brief Return if this is a fixed wing */
......
src/ui/px4_configuration/PX4RCCalibration.cc
View file @ 6235763f
......@@ -892,7 +892,7 @@ void PX4RCCalibration::_writeCalibration(void)
{
if (!_mav) return;
_mav->endRadioControlCalibration();
_mav->stopCalibration();
_validateCalibration();
......@@ -986,7 +986,7 @@ void PX4RCCalibration::_startCalibration(void)
_resetInternalCalibrationValues();
// Let the mav known we are starting calibration. This should turn off motors and so forth.
_mav->startRadioControlCalibration();
_mav->startCalibration(UASInterface::StartCalibrationRadio);
_ui->rcCalNext->setText(tr("Next"));
_ui->rcCalCancel->setEnabled(true);
......@@ -1001,7 +1001,7 @@ void PX4RCCalibration::_stopCalibration(void)
_currentStep = -1;
if (_mav) {
_mav->endRadioControlCalibration();
_mav->stopCalibration();
_setInternalCalibrationValuesFromParameters();
} else {
_resetInternalCalibrationValues();
......
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