Skip to content
Projects
Groups
Snippets
Help
Loading...
Help
Support
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Q
qgroundcontrol
Project
Project
Details
Activity
Releases
Cycle Analytics
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Charts
Issues
0
Issues
0
List
Boards
Labels
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Charts
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Charts
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Valentin Platzgummer
qgroundcontrol
Commits
9d4088b0
Commit
9d4088b0
authored
Aug 20, 2020
by
DonLakeFlyer
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
parent
17e713b5
Changes
8
Show whitespace changes
Inline
Side-by-side
Showing
8 changed files
with
113 additions
and
130 deletions
+113
-130
RadioComponentController.cc
src/AutoPilotPlugins/Common/RadioComponentController.cc
+4
-4
PowerComponentController.cc
src/AutoPilotPlugins/PX4/PowerComponentController.cc
+1
-5
SensorsComponentController.cc
src/AutoPilotPlugins/PX4/SensorsComponentController.cc
+6
-6
Vehicle.cc
src/Vehicle/Vehicle.cc
+83
-2
Vehicle.h
src/Vehicle/Vehicle.h
+19
-1
UAS.cc
src/uas/UAS.cc
+0
-89
UAS.h
src/uas/UAS.h
+0
-3
UASInterface.h
src/uas/UASInterface.h
+0
-20
No files found.
src/AutoPilotPlugins/Common/RadioComponentController.cc
View file @
9d4088b0
...
...
@@ -701,7 +701,7 @@ void RadioComponentController::_writeCalibration(void)
if
(
!
_uas
)
return
;
if
(
_px4Vehicle
())
{
_
uas
->
stopCalibration
();
_
vehicle
->
stopCalibration
();
}
if
(
!
_px4Vehicle
()
&&
(
_vehicle
->
vehicleType
()
==
MAV_TYPE_HELICOPTER
||
_vehicle
->
multiRotor
())
&&
_rgChannelInfo
[
_rgFunctionChannelMapping
[
rcCalFunctionThrottle
]].
reversed
)
{
...
...
@@ -798,7 +798,7 @@ void RadioComponentController::_startCalibration(void)
// Let the mav known we are starting calibration. This should turn off motors and so forth.
if
(
_px4Vehicle
())
{
_
uas
->
startCalibration
(
UASInterface
::
Start
CalibrationRadio
);
_
vehicle
->
startCalibration
(
Vehicle
::
CalibrationRadio
);
}
_nextButton
->
setProperty
(
"text"
,
tr
(
"Next"
));
...
...
@@ -815,7 +815,7 @@ void RadioComponentController::_stopCalibration(void)
if
(
_uas
)
{
if
(
_px4Vehicle
())
{
_
uas
->
stopCalibration
();
_
vehicle
->
stopCalibration
();
}
_setInternalCalibrationValuesFromParameters
();
}
else
{
...
...
@@ -1024,7 +1024,7 @@ void RadioComponentController::_signalAllAttitudeValueChanges(void)
void
RadioComponentController
::
copyTrims
(
void
)
{
_
uas
->
startCalibration
(
UASInterface
::
Start
CalibrationCopyTrims
);
_
vehicle
->
startCalibration
(
Vehicle
::
CalibrationCopyTrims
);
}
bool
RadioComponentController
::
_px4Vehicle
(
void
)
const
...
...
src/AutoPilotPlugins/PX4/PowerComponentController.cc
View file @
9d4088b0
...
...
@@ -7,10 +7,6 @@
*
****************************************************************************/
/// @file
/// @author Don Gagne <don@thegagnes.com>
#include "PowerComponentController.h"
#include "QGCMAVLink.h"
#include "UAS.h"
...
...
@@ -27,7 +23,7 @@ void PowerComponentController::calibrateEsc(void)
{
_warningMessages
.
clear
();
connect
(
_vehicle
,
&
Vehicle
::
textMessageReceived
,
this
,
&
PowerComponentController
::
_handleUASTextMessage
);
_
uas
->
startCalibration
(
UASInterface
::
Start
CalibrationEsc
);
_
vehicle
->
startCalibration
(
Vehicle
::
CalibrationEsc
);
}
void
PowerComponentController
::
busConfigureActuators
(
void
)
...
...
src/AutoPilotPlugins/PX4/SensorsComponentController.cc
View file @
9d4088b0
...
...
@@ -192,31 +192,31 @@ void SensorsComponentController::_stopCalibration(SensorsComponentController::St
void
SensorsComponentController
::
calibrateGyro
(
void
)
{
_startLogCalibration
();
_
uas
->
startCalibration
(
UASInterface
::
Start
CalibrationGyro
);
_
vehicle
->
startCalibration
(
Vehicle
::
CalibrationGyro
);
}
void
SensorsComponentController
::
calibrateCompass
(
void
)
{
_startLogCalibration
();
_
uas
->
startCalibration
(
UASInterface
::
Start
CalibrationMag
);
_
vehicle
->
startCalibration
(
Vehicle
::
CalibrationMag
);
}
void
SensorsComponentController
::
calibrateAccel
(
void
)
{
_startLogCalibration
();
_
uas
->
startCalibration
(
UASInterface
::
Start
CalibrationAccel
);
_
vehicle
->
startCalibration
(
Vehicle
::
CalibrationAccel
);
}
void
SensorsComponentController
::
calibrateLevel
(
void
)
{
_startLogCalibration
();
_
uas
->
startCalibration
(
UASInterface
::
Start
CalibrationLevel
);
_
vehicle
->
startCalibration
(
Vehicle
::
CalibrationLevel
);
}
void
SensorsComponentController
::
calibrateAirspeed
(
void
)
{
_startLogCalibration
();
_
uas
->
startCalibration
(
UASInterface
::
StartCalibration
Airspeed
);
_
vehicle
->
startCalibration
(
Vehicle
::
CalibrationPX4
Airspeed
);
}
void
SensorsComponentController
::
_handleUASTextMessage
(
int
uasId
,
int
compId
,
int
severity
,
QString
text
)
...
...
@@ -485,5 +485,5 @@ void SensorsComponentController::cancelCalibration(void)
_waitingForCancel
=
true
;
emit
waitingForCancelChanged
();
_cancelButton
->
setEnabled
(
false
);
_
uas
->
stopCalibration
();
_
vehicle
->
stopCalibration
();
}
src/Vehicle/Vehicle.cc
View file @
9d4088b0
...
...
@@ -3583,6 +3583,87 @@ void Vehicle::rebootVehicle()
sendMessageOnLinkThreadSafe
(
priorityLink
(),
msg
);
}
void
Vehicle
::
startCalibration
(
Vehicle
::
CalibrationType
calType
)
{
float
param1
=
0
;
float
param2
=
0
;
float
param3
=
0
;
float
param4
=
0
;
float
param5
=
0
;
float
param6
=
0
;
float
param7
=
0
;
switch
(
calType
)
{
case
CalibrationGyro
:
param1
=
1
;
break
;
case
CalibrationMag
:
param2
=
1
;
break
;
case
CalibrationRadio
:
param4
=
1
;
break
;
case
CalibrationCopyTrims
:
param4
=
2
;
break
;
case
CalibrationAccel
:
param5
=
1
;
break
;
case
CalibrationLevel
:
param5
=
2
;
break
;
case
CalibrationEsc
:
param7
=
1
;
break
;
case
CalibrationPX4Airspeed
:
param6
=
1
;
break
;
case
CalibrationPX4Pressure
:
param3
=
1
;
break
;
case
CalibrationAPMCompassMot
:
param6
=
1
;
break
;
case
CalibrationAPMPressureAirspeed
:
param3
=
1
;
break
;
case
CalibrationAPMPreFlight
:
param3
=
1
;
// GroundPressure/Airspeed
if
(
multiRotor
()
||
rover
())
{
// Gyro cal for ArduCopter only
param1
=
1
;
}
}
// We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn
// causes the retry logic to break down.
mavlink_message_t
msg
;
mavlink_msg_command_long_pack_chan
(
_mavlink
->
getSystemId
(),
_mavlink
->
getComponentId
(),
priorityLink
()
->
mavlinkChannel
(),
&
msg
,
id
(),
defaultComponentId
(),
// target component
MAV_CMD_PREFLIGHT_CALIBRATION
,
// command id
0
,
// 0=first transmission of command
param1
,
param2
,
param3
,
param4
,
param5
,
param6
,
param7
);
sendMessageOnLinkThreadSafe
(
priorityLink
(),
msg
);
}
void
Vehicle
::
stopCalibration
(
void
)
{
sendMavCommand
(
defaultComponentId
(),
// target component
MAV_CMD_PREFLIGHT_CALIBRATION
,
// command id
true
,
// showError
0
,
// gyro cal
0
,
// mag cal
0
,
// ground pressure
0
,
// radio cal
0
,
// accel cal
0
,
// airspeed cal
0
);
// unused
}
void
Vehicle
::
setSoloFirmware
(
bool
soloFirmware
)
{
if
(
soloFirmware
!=
_soloFirmware
)
{
...
...
src/Vehicle/Vehicle.h
View file @
9d4088b0
...
...
@@ -641,7 +641,7 @@ public:
Q_PROPERTY
(
qreal
gimbalPitch
READ
gimbalPitch
NOTIFY
gimbalPitchChanged
)
Q_PROPERTY
(
qreal
gimbalYaw
READ
gimbalYaw
NOTIFY
gimbalYawChanged
)
Q_PROPERTY
(
bool
gimbalData
READ
gimbalData
NOTIFY
gimbalDataChanged
)
Q_PROPERTY
(
bool
isROIEnabled
READ
isROIEnabled
NOTIFY
isROIEnabledChanged
)
Q_PROPERTY
(
bool
i
ARDUR
sROIEnabled
READ
isROIEnabled
NOTIFY
isROIEnabledChanged
)
Q_PROPERTY
(
CheckList
checkListState
READ
checkListState
WRITE
setCheckListState
NOTIFY
checkListStateChanged
)
Q_PROPERTY
(
bool
readyToFlyAvailable
READ
readyToFlyAvailable
NOTIFY
readyToFlyAvailableChanged
)
///< true: readyToFly signalling is available on this vehicle
Q_PROPERTY
(
bool
readyToFly
READ
readyToFly
NOTIFY
readyToFlyChanged
)
...
...
@@ -980,6 +980,24 @@ public:
/// @return the maximum version
unsigned
maxProtoVersion
()
const
{
return
_maxProtoVersion
;
}
enum
CalibrationType
{
CalibrationRadio
,
CalibrationGyro
,
CalibrationMag
,
CalibrationAccel
,
CalibrationLevel
,
CalibrationEsc
,
CalibrationCopyTrims
,
CalibrationAPMCompassMot
,
CalibrationAPMPressureAirspeed
,
CalibrationAPMPreFlight
,
CalibrationPX4Airspeed
,
CalibrationPX4Pressure
,
};
void
startCalibration
(
CalibrationType
calType
);
void
stopCalibration
(
void
);
Fact
*
roll
()
{
return
&
_rollFact
;
}
Fact
*
pitch
()
{
return
&
_pitchFact
;
}
Fact
*
heading
()
{
return
&
_headingFact
;
}
...
...
src/uas/UAS.cc
View file @
9d4088b0
...
...
@@ -348,95 +348,6 @@ void UAS::receiveMessage(mavlink_message_t message)
#pragma warning(pop, 0)
#endif
void
UAS
::
startCalibration
(
UASInterface
::
StartCalibrationType
calType
)
{
if
(
!
_vehicle
)
{
return
;
}
int
gyroCal
=
0
;
int
magCal
=
0
;
int
airspeedCal
=
0
;
int
radioCal
=
0
;
int
accelCal
=
0
;
int
pressureCal
=
0
;
int
escCal
=
0
;
switch
(
calType
)
{
case
StartCalibrationGyro
:
gyroCal
=
1
;
break
;
case
StartCalibrationMag
:
magCal
=
1
;
break
;
case
StartCalibrationAirspeed
:
airspeedCal
=
1
;
break
;
case
StartCalibrationRadio
:
radioCal
=
1
;
break
;
case
StartCalibrationCopyTrims
:
radioCal
=
2
;
break
;
case
StartCalibrationAccel
:
accelCal
=
1
;
break
;
case
StartCalibrationLevel
:
accelCal
=
2
;
break
;
case
StartCalibrationPressure
:
pressureCal
=
1
;
break
;
case
StartCalibrationEsc
:
escCal
=
1
;
break
;
case
StartCalibrationUavcanEsc
:
escCal
=
2
;
break
;
case
StartCalibrationCompassMot
:
airspeedCal
=
1
;
// ArduPilot, bit of a hack
break
;
}
// We can't use sendMavCommand here since we have no idea how long it will be before the command returns a result. This in turn
// causes the retry logic to break down.
mavlink_message_t
msg
;
mavlink_msg_command_long_pack_chan
(
mavlink
->
getSystemId
(),
mavlink
->
getComponentId
(),
_vehicle
->
priorityLink
()
->
mavlinkChannel
(),
&
msg
,
uasId
,
_vehicle
->
defaultComponentId
(),
// target component
MAV_CMD_PREFLIGHT_CALIBRATION
,
// command id
0
,
// 0=first transmission of command
gyroCal
,
// gyro cal
magCal
,
// mag cal
pressureCal
,
// ground pressure
radioCal
,
// radio cal
accelCal
,
// accel cal
airspeedCal
,
// PX4: airspeed cal, ArduPilot: compass mot
escCal
);
// esc cal
_vehicle
->
sendMessageOnLinkThreadSafe
(
_vehicle
->
priorityLink
(),
msg
);
}
void
UAS
::
stopCalibration
(
void
)
{
if
(
!
_vehicle
)
{
return
;
}
_vehicle
->
sendMavCommand
(
_vehicle
->
defaultComponentId
(),
// target component
MAV_CMD_PREFLIGHT_CALIBRATION
,
// command id
true
,
// showError
0
,
// gyro cal
0
,
// mag cal
0
,
// ground pressure
0
,
// radio cal
0
,
// accel cal
0
,
// airspeed cal
0
);
// unused
}
void
UAS
::
startBusConfig
(
UASInterface
::
StartBusConfigType
calType
)
{
if
(
!
_vehicle
)
{
...
...
src/uas/UAS.h
View file @
9d4088b0
...
...
@@ -180,9 +180,6 @@ public slots:
/** @brief Receive a message from one of the communication links. */
virtual
void
receiveMessage
(
mavlink_message_t
message
);
void
startCalibration
(
StartCalibrationType
calType
);
void
stopCalibration
(
void
);
void
startBusConfig
(
StartBusConfigType
calType
);
void
stopBusConfig
(
void
);
...
...
src/uas/UASInterface.h
View file @
9d4088b0
...
...
@@ -39,31 +39,11 @@ public:
/** @brief The time interval the robot is switched on **/
virtual
quint64
getUptime
()
const
=
0
;
enum
StartCalibrationType
{
StartCalibrationRadio
,
StartCalibrationGyro
,
StartCalibrationMag
,
StartCalibrationAirspeed
,
StartCalibrationAccel
,
StartCalibrationLevel
,
StartCalibrationPressure
,
StartCalibrationEsc
,
StartCalibrationCopyTrims
,
StartCalibrationUavcanEsc
,
StartCalibrationCompassMot
,
};
enum
StartBusConfigType
{
StartBusConfigActuators
,
EndBusConfigActuators
,
};
/// Starts the specified calibration
virtual
void
startCalibration
(
StartCalibrationType
calType
)
=
0
;
/// Ends any current calibration
virtual
void
stopCalibration
(
void
)
=
0
;
/// Starts the specified bus configuration
virtual
void
startBusConfig
(
StartBusConfigType
calType
)
=
0
;
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment