Skip to content

Commit 1f76180

Browse files
kpetrykinkpetrykin
committed
Adds a possibility of sending MAV_DO_SET_MODE with parameters for Charging Station vehicle type
1 parent e639b1d commit 1f76180

5 files changed

Lines changed: 274 additions & 19 deletions

File tree

‎src/FirmwarePlugin/ChargingStation/ChargingStationFirmwarePlugin.cc‎

Lines changed: 39 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ QStringList ChargingStationFirmwarePlugin::flightModes(Vehicle* vehicle)
3131
Q_UNUSED(vehicle);
3232

3333
QStringList flightModes;
34-
flightModes << tr("Open") << tr("Closed");
34+
flightModes << tr("Open") << tr("Closed") << tr("Load drone") << tr("Unload drone") << tr("Get from user") << tr("Unload to user") << tr("Change battery") << tr("Reset") << tr("Service");
3535
return flightModes;
3636
}
3737

@@ -43,6 +43,20 @@ bool ChargingStationFirmwarePlugin::setFlightMode(const QString& flightMode, uin
4343
*custom_mode = CUSTOM_MODE_OPEN;
4444
} else if (flightMode.compare(tr("Closed"), Qt::CaseInsensitive) == 0) {
4545
*custom_mode = CUSTOM_MODE_CLOSED;
46+
} else if (flightMode.compare(tr("Load drone"), Qt::CaseInsensitive) == 0) {
47+
*custom_mode = CUSTOM_MODE_LOADING_DRONE;
48+
} else if (flightMode.compare(tr("Unload drone"), Qt::CaseInsensitive) == 0) {
49+
*custom_mode = CUSTOM_MODE_UNLOADING_DRONE;
50+
} else if (flightMode.compare(tr("Get from user"), Qt::CaseInsensitive) == 0) {
51+
*custom_mode = CUSTOM_MODE_GETTING_FROM_USER;
52+
} else if (flightMode.compare(tr("Unload to user"), Qt::CaseInsensitive) == 0) {
53+
*custom_mode = CUSTOM_MODE_UNLOADING_TO_USER;
54+
} else if (flightMode.compare(tr("Change battery"), Qt::CaseInsensitive) == 0) {
55+
*custom_mode = CUSTOM_MODE_CHANGING_BATTERY;
56+
} else if (flightMode.compare(tr("Reset"), Qt::CaseInsensitive) == 0) {
57+
*custom_mode = CUSTOM_MODE_RESET;
58+
} else if (flightMode.compare(tr("Service"), Qt::CaseInsensitive) == 0) {
59+
*custom_mode = CUSTOM_MODE_SERVICE;
4660
} else {
4761
return false;
4862
}
@@ -54,14 +68,30 @@ QString ChargingStationFirmwarePlugin::flightMode(uint8_t base_mode, uint32_t cu
5468
{
5569
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
5670
switch (custom_mode) {
57-
case CUSTOM_MODE_OPEN:
58-
return tr("Open");
59-
case CUSTOM_MODE_OPENING:
60-
return tr("Opening");
61-
case CUSTOM_MODE_CLOSED:
62-
return tr("Closed");
63-
case CUSTOM_MODE_CLOSING:
64-
return tr("Closing");
71+
case CUSTOM_MODE_OPEN:
72+
return tr("Open");
73+
case CUSTOM_MODE_OPENING:
74+
return tr("Opening");
75+
case CUSTOM_MODE_CLOSED:
76+
return tr("Closed");
77+
case CUSTOM_MODE_CLOSING:
78+
return tr("Closing");
79+
case CUSTOM_MODE_LOADING_DRONE:
80+
return tr("Loading drone");
81+
case CUSTOM_MODE_UNLOADING_DRONE:
82+
return tr("Unloading drone");
83+
case CUSTOM_MODE_GETTING_FROM_USER:
84+
return tr("Getting from user");
85+
case CUSTOM_MODE_UNLOADING_TO_USER:
86+
return tr("Unloading to user");
87+
case CUSTOM_MODE_CHANGING_BATTERY:
88+
return tr("Changing battery");
89+
case CUSTOM_MODE_SERVICE:
90+
return tr("Service");
91+
case CUSTOM_MODE_ERROR:
92+
return tr("Error");
93+
case CUSTOM_MODE_STANDBY:
94+
return tr("Standby");
6595
}
6696
} else {
6797
qWarning() << "Charging station mode without custom mode enabled";

‎src/FirmwarePlugin/ChargingStation/ChargingStationFirmwarePlugin.h‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,16 @@
88
#define CUSTOM_MODE_OPENING 2
99
#define CUSTOM_MODE_CLOSED 3
1010
#define CUSTOM_MODE_CLOSING 4
11+
#define CUSTOM_MODE_LOADING_DRONE 5
12+
#define CUSTOM_MODE_UNLOADING_DRONE 6
13+
#define CUSTOM_MODE_GETTING_FROM_USER 7
14+
#define CUSTOM_MODE_UNLOADING_TO_USER 8
15+
#define CUSTOM_MODE_CHANGING_BATTERY 9
16+
#define CUSTOM_MODE_SERVICE 10
17+
#define CUSTOM_MODE_RESET 11
18+
#define CUSTOM_MODE_STANDBY 12
19+
#define CUSTOM_MODE_ERROR 13
20+
1121

1222
class ChargingStationFactGroup : public FactGroup
1323
{

‎src/Vehicle/Vehicle.cc‎

Lines changed: 61 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -2437,6 +2437,38 @@ QString Vehicle::flightMode(void) const
24372437
return _firmwarePlugin->flightMode(_base_mode, _custom_mode);
24382438
}
24392439

2440+
void Vehicle::setFlightModeParametrized(const QString& flightMode, float param1, float param2, float param3, float param4, float param5)
2441+
{
2442+
qDebug() << "Received the following! " << flightMode << " " << param1 << " " << param2 << " " << param3 << " " << param4 << " " << param5;
2443+
if (vehicleType() == MAV_TYPE_CHARGING_STATION)
2444+
{
2445+
uint8_t base_mode;
2446+
uint32_t custom_mode;
2447+
2448+
if (_firmwarePlugin->setFlightMode(flightMode, &base_mode, &custom_mode)) {
2449+
// setFlightMode will only set MAV_MODE_FLAG_CUSTOM_MODE_ENABLED in base_mode, we need to move back in the existing
2450+
// states.
2451+
uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE;
2452+
newBaseMode |= base_mode;
2453+
2454+
// MAV_CMD_DO_SET_MODE
2455+
sendMavCommand(_defaultComponentId,
2456+
MAV_CMD_DO_SET_MODE,
2457+
true, // show errors
2458+
newBaseMode, // Mode
2459+
custom_mode, // Cusnom mode
2460+
param1, // Custom submode
2461+
param2,
2462+
param3,
2463+
param4,
2464+
param5);
2465+
}
2466+
}
2467+
else
2468+
setFlightMode(flightMode);
2469+
2470+
}
2471+
24402472
void Vehicle::setFlightMode(const QString& flightMode)
24412473
{
24422474
uint8_t base_mode;
@@ -2448,15 +2480,34 @@ void Vehicle::setFlightMode(const QString& flightMode)
24482480
uint8_t newBaseMode = _base_mode & ~MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE;
24492481
newBaseMode |= base_mode;
24502482

2451-
mavlink_message_t msg;
2452-
mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
2453-
_mavlink->getComponentId(),
2454-
priorityLink()->mavlinkChannel(),
2455-
&msg,
2456-
id(),
2457-
newBaseMode,
2458-
custom_mode);
2459-
sendMessageOnLink(priorityLink(), msg);
2483+
if (vehicleType() == MAV_TYPE_CHARGING_STATION)
2484+
{
2485+
qDebug() << "Sending status to dronopoint!";
2486+
2487+
// MAV_CMD_DO_SET_MODE
2488+
sendMavCommand(_defaultComponentId,
2489+
MAV_CMD_DO_SET_MODE,
2490+
true, // show errors
2491+
newBaseMode, // Mode
2492+
custom_mode, // Cusnom mode
2493+
0.0, // Custom submode
2494+
0.0,
2495+
0.0,
2496+
0.0,
2497+
0.0);
2498+
}
2499+
else {
2500+
mavlink_message_t msg;
2501+
mavlink_msg_set_mode_pack_chan(_mavlink->getSystemId(),
2502+
_mavlink->getComponentId(),
2503+
priorityLink()->mavlinkChannel(),
2504+
&msg,
2505+
id(),
2506+
newBaseMode,
2507+
custom_mode);
2508+
sendMessageOnLink(priorityLink(), msg);
2509+
}
2510+
24602511
} else {
24612512
qWarning() << "FirmwarePlugin::setFlightMode failed, flightMode:" << flightMode;
24622513
}
@@ -2994,6 +3045,7 @@ QString Vehicle::vehicleTypeName() const {
29943045
{ MAV_TYPE_VTOL_RESERVED5, tr("VTOL reserved 5")},
29953046
{ MAV_TYPE_GIMBAL, tr("Onboard gimbal")},
29963047
{ MAV_TYPE_ADSB, tr("Onboard ADSB peripheral")},
3048+
{ MAV_TYPE_CHARGING_STATION,tr("Charging station")}
29973049
};
29983050
return typeNames[_vehicleType];
29993051
}

‎src/Vehicle/Vehicle.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -820,6 +820,8 @@ class Vehicle : public FactGroup
820820
Q_INVOKABLE void processCameraProperty(float commandType, QString propertyName, float value);
821821
Q_INVOKABLE void sendPlan(QString planFile);
822822

823+
Q_INVOKABLE void setFlightModeParametrized(const QString& flightMode, float param1, float param2, float param3, float param4, float param5);
824+
823825
/// Used to check if running current version is equal or higher than the one being compared.
824826
// returns 1 if current > compare, 0 if current == compare, -1 if current < compare
825827
Q_INVOKABLE int versionCompare(QString& compare);

‎src/ui/toolbar/ModeIndicator.qml‎

Lines changed: 162 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,11 @@ Item {
2525
width: flightModeSelector.width
2626

2727
property var _flightModes: activeVehicle ? activeVehicle.flightModes : [ ]
28+
property var modeParam1: 0
29+
property var modeParam2: 0
30+
property var modeParam3: 0
31+
property var modeParam4: 0
32+
property var modeParam5: 0
2833

2934
on_FlightModesChanged: flightModeSelector.updateFlightModesMenu()
3035

@@ -43,6 +48,156 @@ Item {
4348
onTriggered: activeVehicle.flightMode = text
4449
}
4550
}
51+
Component {
52+
id: parametrizedModeSelector
53+
Rectangle {
54+
id: testRect
55+
width: 200
56+
height: 300
57+
radius: ScreenTools.defaultFontPixelHeight * 0.5
58+
color: qgcPal.window
59+
border.color: qgcPal.text
60+
Column{
61+
width: parent.width * 0.9
62+
anchors.horizontalCenter: parent.horizontalCenter
63+
ComboBox {
64+
id: cbFlightMode
65+
currentIndex: -1
66+
displayText: currentIndex === -1 ? "Choose mode..." : currentText
67+
model: ListModel {
68+
id: cbFlightModeModel
69+
}
70+
width: parent.width
71+
anchors.horizontalCenter: parent.horizontalCenter
72+
Component.onCompleted: {
73+
cbFlightModeModel.clear()
74+
var i
75+
for (i = 0; i < _flightModes.length; i++) {
76+
cbFlightModeModel.append({'mode': _flightModes[i]})
77+
}
78+
currentIndex = find(activeVehicle.flightMode)
79+
}
80+
}
81+
Row {
82+
width: parent.width
83+
QGCLabel {
84+
text: qsTr("Param1: ")
85+
}
86+
TextField {
87+
id: param1Box
88+
text: modeParam1
89+
width: parent.width * 0.5
90+
anchors.verticalCenter: parent.verticalCenter
91+
inputMethodHints: Qt.ImhDigitsOnly
92+
validator: IntValidator {
93+
bottom: -32768
94+
top: 32768
95+
}
96+
onAccepted: {
97+
modeParam1 = text
98+
}
99+
}
100+
}
101+
Row {
102+
width: parent.width
103+
QGCLabel {
104+
text: qsTr("Param2: ")
105+
}
106+
TextField {
107+
id: param2Box
108+
text: modeParam2
109+
width: parent.width * 0.5
110+
anchors.verticalCenter: parent.verticalCenter
111+
inputMethodHints: Qt.ImhDigitsOnly
112+
validator: IntValidator {
113+
bottom: -32768
114+
top: 32768
115+
}
116+
onAccepted: {
117+
modeParam2 = text
118+
}
119+
}
120+
}
121+
Row {
122+
width: parent.width
123+
QGCLabel {
124+
text: qsTr("Param3: ")
125+
}
126+
TextField {
127+
id: param3Box
128+
text: modeParam3
129+
width: parent.width * 0.5
130+
anchors.verticalCenter: parent.verticalCenter
131+
inputMethodHints: Qt.ImhDigitsOnly
132+
validator: IntValidator {
133+
bottom: -32768
134+
top: 32768
135+
}
136+
onAccepted: {
137+
modeParam3 = text
138+
}
139+
}
140+
}
141+
Row {
142+
width: parent.width
143+
QGCLabel {
144+
text: qsTr("Param4: ")
145+
}
146+
TextField {
147+
id: param4Box
148+
text: modeParam4
149+
width: parent.width * 0.5
150+
anchors.verticalCenter: parent.verticalCenter
151+
inputMethodHints: Qt.ImhDigitsOnly
152+
validator: IntValidator {
153+
bottom: -32768
154+
top: 32768
155+
}
156+
onAccepted: {
157+
modeParam4 = text
158+
}
159+
}
160+
}
161+
Row {
162+
width: parent.width
163+
QGCLabel {
164+
text: qsTr("Param5: ")
165+
}
166+
TextField {
167+
id: param5Box
168+
text: modeParam5
169+
width: parent.width * 0.5
170+
anchors.verticalCenter: parent.verticalCenter
171+
inputMethodHints: Qt.ImhDigitsOnly
172+
validator: IntValidator {
173+
bottom: -32768
174+
top: 32768
175+
}
176+
onAccepted: {
177+
modeParam5 = text
178+
}
179+
}
180+
}
181+
QGCButton {
182+
anchors.horizontalCenter: parent.horizontalCenter
183+
text: qsTr("Change mode")
184+
onClicked: {
185+
activeVehicle.setFlightModeParametrized(
186+
cbFlightMode.currentText,
187+
param1Box.text ? parseFloat(param1Box.text) : 0,
188+
param2Box.text ? parseFloat(param2Box.text) : 0,
189+
param3Box.text ? parseFloat(param3Box.text) : 0,
190+
param4Box.text ? parseFloat(param4Box.text) : 0,
191+
param5Box.text ? parseFloat(param5Box.text) : 0
192+
)
193+
}
194+
}
195+
}
196+
197+
198+
}
199+
200+
}
46201
property var flightModesMenuItems: []
47202
function updateFlightModesMenu() {
48203
if (activeVehicle && activeVehicle.flightModeSetAvailable) {
@@ -64,7 +219,13 @@ Item {
64219
MouseArea {
65220
visible: activeVehicle && activeVehicle.flightModeSetAvailable
66221
anchors.fill: parent
67-
onClicked: flightModesMenu.popup()
222+
// onClicked: mainWindow.showPopUp(flightModeSelector, parametrizedModeSelector)
223+
onClicked: {
224+
if (activeVehicle.vehicleTypeName() == "Charging station")
225+
mainWindow.showPopUp(flightModeSelector, parametrizedModeSelector)
226+
else
227+
flightModesMenu.popup()
228+
}
68229
}
69230
}
70231
}

0 commit comments

Comments
 (0)