-
Notifications
You must be signed in to change notification settings - Fork 4.6k
/
Copy pathMavLinkMultirotorApi.hpp
1656 lines (1432 loc) · 62.9 KB
/
MavLinkMultirotorApi.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License.
#ifndef msr_airlib_MavLinkDroneController_hpp
#define msr_airlib_MavLinkDroneController_hpp
#include "MavLinkVehicle.hpp"
#include "MavLinkConnection.hpp"
#include "MavLinkMessages.hpp"
#include "MavLinkNode.hpp"
#include "MavLinkVideoStream.hpp"
#include <queue>
#include <mutex>
#include <string>
#include <vector>
#include <memory>
#include <exception>
#include <thread>
#include <tuple>
#include "common/Common.hpp"
#include "common/common_utils/SmoothingFilter.hpp"
#include "common/common_utils/Timer.hpp"
#include "common/CommonStructs.hpp"
#include "common/VectorMath.hpp"
#include "common/AirSimSettings.hpp"
#include "vehicles/multirotor/api/MultirotorApiBase.hpp"
#include "common/PidController.hpp"
#include "sensors/SensorCollection.hpp"
//sensors
#include "sensors/barometer/BarometerBase.hpp"
#include "sensors/imu/ImuBase.hpp"
#include "sensors/gps/GpsBase.hpp"
#include "sensors/magnetometer/MagnetometerBase.hpp"
#include "sensors/distance/DistanceSimple.hpp"
namespace msr { namespace airlib {
class MavLinkMultirotorApi : public MultirotorApiBase
{
public: //methods
virtual ~MavLinkMultirotorApi()
{
closeAllConnection();
if (this->connect_thread_.joinable())
{
this->connect_thread_.join();
}
}
//non-base interface specific to MavLinKDroneController
void initialize(const AirSimSettings::MavLinkConnectionInfo& connection_info, const SensorCollection* sensors, bool is_simulation)
{
connection_info_ = connection_info;
sensors_ = sensors;
is_simulation_mode_ = is_simulation;
try {
openAllConnections();
is_ready_ = true;
}
catch (std::exception& ex) {
is_ready_ = false;
is_ready_message_ = Utils::stringf("Failed to connect: %s", ex.what());
}
}
Pose getMocapPose()
{
std::lock_guard<std::mutex> guard(mocap_pose_mutex_);
return mocap_pose_;
}
virtual const SensorCollection& getSensors() const override
{
return *sensors_;
}
//reset PX4 stack
virtual void resetImplementation() override
{
MultirotorApiBase::resetImplementation();
resetState();
was_reset_ = true;
setNormalMode();
}
//update sensors in PX4 stack
virtual void update() override
{
try {
MultirotorApiBase::update();
if (sensors_ == nullptr || !connected_ || connection_ == nullptr || !connection_->isOpen() || !got_first_heartbeat_)
return;
if (send_params_) {
send_params_ = false;
sendParams();
}
//send sensor updates
const auto& imu_output = getImuData("");
const auto& mag_output = getMagnetometerData("");
const auto& baro_output = getBarometerData("");
sendHILSensor(imu_output.linear_acceleration,
imu_output.angular_velocity,
mag_output.magnetic_field_body,
baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude);
const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
if (count_distance_sensors != 0) {
const auto* distance_sensor = static_cast<const DistanceSimple*>(
sensors_->getByType(SensorBase::SensorType::Distance));
// Don't send the data if sending to external controller is disabled in settings
if (distance_sensor && distance_sensor->getParams().external_controller) {
const auto& distance_output = distance_sensor->getOutput();
sendDistanceSensor(distance_output.min_distance * 100, //m -> cm
distance_output.max_distance * 100, //m -> cm
distance_output.distance * 100, //m-> cm
0, //sensor type: //TODO: allow changing in settings?
77, //sensor id, //TODO: should this be something real?
distance_output.relative_pose.orientation); //TODO: convert from radians to degrees?
}
}
const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps);
if (count_gps_sensors != 0) {
const auto& gps_output = getGpsData("");
//send GPS
if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) {
last_gps_time_ = gps_output.gnss.time_utc;
Vector3r gps_velocity = gps_output.gnss.velocity;
Vector3r gps_velocity_xy = gps_velocity;
gps_velocity_xy.z() = 0;
float gps_cog;
if (Utils::isApproximatelyZero(gps_velocity.y(), 1E-2f) && Utils::isApproximatelyZero(gps_velocity.x(), 1E-2f))
gps_cog = 0;
else
gps_cog = Utils::radiansToDegrees(atan2(gps_velocity.y(), gps_velocity.x()));
if (gps_cog < 0)
gps_cog += 360;
sendHILGps(gps_output.gnss.geo_point, gps_velocity, gps_velocity_xy.norm(), gps_cog,
gps_output.gnss.eph, gps_output.gnss.epv, gps_output.gnss.fix_type, 10);
}
}
}
catch (std::exception& e) {
addStatusMessage("Exception sending messages to vehicle");
addStatusMessage(e.what());
disconnect();
connect(); // re-start a new connection so PX4 can be restarted and AirSim will happily continue on.
}
//must be done at the end
if (was_reset_)
was_reset_ = false;
}
virtual bool isReady(std::string& message) const override
{
if (!is_ready_ && is_ready_message_.size() > 0) {
message = is_ready_message_;
}
return is_ready_;
}
virtual bool canArm() const override
{
return is_ready_ && has_gps_lock_;
}
//TODO: this method can't be const yet because it clears previous messages
virtual void getStatusMessages(std::vector<std::string>& messages) override
{
updateState();
//clear param
messages.clear();
//move messages from private vars to param
std::lock_guard<std::mutex> guard(status_text_mutex_);
while (!status_messages_.empty()) {
messages.push_back(status_messages_.front());
status_messages_.pop();
}
}
virtual Kinematics::State getKinematicsEstimated() const override
{
updateState();
Kinematics::State state;
//TODO: reduce code duplication in getPosition() etc methods?
state.pose.position = Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
state.pose.orientation = VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
state.twist.linear = Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
state.twist.angular = Vector3r(current_state_.attitude.roll_rate, current_state_.attitude.pitch_rate, current_state_.attitude.yaw_rate);
state.accelerations.linear = Vector3r(current_state_.local_est.acc.x, current_state_.local_est.acc.y, current_state_.local_est.acc.z);
//TODO: how do we get angular acceleration?
return state;
}
virtual bool isApiControlEnabled() const override
{
return is_api_control_enabled_;
}
virtual void enableApiControl(bool is_enabled) override
{
checkValidVehicle();
if (is_enabled) {
mav_vehicle_->requestControl();
is_api_control_enabled_ = true;
}
else {
mav_vehicle_->releaseControl();
is_api_control_enabled_ = false;
}
}
virtual Vector3r getPosition() const override
{
updateState();
return Vector3r(current_state_.local_est.pos.x, current_state_.local_est.pos.y, current_state_.local_est.pos.z);
}
virtual Vector3r getVelocity() const override
{
updateState();
return Vector3r(current_state_.local_est.lin_vel.x, current_state_.local_est.lin_vel.y, current_state_.local_est.lin_vel.z);
}
virtual Quaternionr getOrientation() const override
{
updateState();
return VectorMath::toQuaternion(current_state_.attitude.pitch, current_state_.attitude.roll, current_state_.attitude.yaw);
}
virtual LandedState getLandedState() const override
{
updateState();
return current_state_.controls.landed ? LandedState::Landed : LandedState::Flying;
}
virtual real_T getActuation(unsigned int rotor_index) const override
{
if (!is_simulation_mode_)
throw std::logic_error("Attempt to read motor controls while not in simulation mode");
std::lock_guard<std::mutex> guard(hil_controls_mutex_);
return rotor_controls_[rotor_index];
}
virtual size_t getActuatorCount() const override
{
return RotorControlsCount;
}
virtual bool armDisarm(bool arm) override
{
SingleCall lock(this);
checkValidVehicle();
bool rc = false;
if (arm) {
float timeout_sec = 10;
waitForHomeLocation(timeout_sec);
waitForStableGroundPosition(timeout_sec);
}
mav_vehicle_->armDisarm(arm).wait(10000, &rc);
return rc;
}
void waitForHomeLocation(float timeout_sec)
{
if (!current_state_.home.is_set) {
addStatusMessage("Waiting for valid GPS home location...");
if (!waitForFunction([&]() {
return current_state_.home.is_set;
}, timeout_sec).isComplete()) {
throw VehicleMoveException("Vehicle does not have a valid GPS home location");
}
}
}
void waitForStableGroundPosition(float timeout_sec)
{
// wait for ground stabilization
if (ground_variance_ > GroundTolerance) {
addStatusMessage("Waiting for z-position to stabilize...");
if (!waitForFunction([&]() {
return ground_variance_ <= GroundTolerance;
}, timeout_sec).isComplete())
{
auto msg = Utils::stringf("Ground is not stable, variance is %f", ground_variance_);
throw VehicleMoveException(msg);
}
}
}
virtual bool takeoff(float timeout_sec) override
{
SingleCall lock(this);
checkValidVehicle();
waitForHomeLocation(timeout_sec);
waitForStableGroundPosition(timeout_sec);
bool rc = false;
auto vec = getPosition();
auto yaw = current_state_.attitude.yaw;
float z = vec.z() + getTakeoffZ();
if (!mav_vehicle_->takeoff(z, 0.0f /* pitch */, yaw).wait(static_cast<int>(timeout_sec * 1000), &rc)) {
throw VehicleMoveException("TakeOff command - timeout waiting for response");
}
if (!rc) {
throw VehicleMoveException("TakeOff command rejected by drone");
}
if (timeout_sec <= 0)
return true; // client doesn't want to wait.
return waitForZ(timeout_sec, z, getDistanceAccuracy());
}
virtual bool land(float timeout_sec) override
{
SingleCall lock(this);
//TODO: bugbug: really need a downward pointing distance to ground sensor to do this properly, for now
//we assume the ground is relatively flat an we are landing roughly at the home altitude.
updateState();
checkValidVehicle();
if (current_state_.home.is_set) {
bool rc = false;
if (!mav_vehicle_->land(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.home.global_pos.alt).wait(10000, &rc))
{
throw VehicleMoveException("Landing command - timeout waiting for response from drone");
}
else if (!rc) {
throw VehicleMoveException("Landing command rejected by drone");
}
}
else {
throw VehicleMoveException("Cannot land safely with out a home position that tells us the home altitude. Could fix this if we hook up a distance to ground sensor...");
}
const auto& waiter = waitForFunction([&]() {
updateState();
return current_state_.controls.landed;
}, timeout_sec);
// Wait for landed state (or user cancellation)
if (!waiter.isComplete())
{
throw VehicleMoveException("Drone hasn't reported a landing state");
}
return waiter.isComplete();
}
virtual bool goHome(float timeout_sec) override
{
SingleCall lock(this);
checkValidVehicle();
bool rc = false;
if (mav_vehicle_ != nullptr && !mav_vehicle_->returnToHome().wait(
static_cast<int>(timeout_sec) * 1000, &rc)) {
throw VehicleMoveException("goHome - timeout waiting for response from drone");
}
return rc;
}
virtual bool moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain,
const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) override
{
SingleTaskCall lock(this);
unused(adaptive_lookahead);
unused(lookahead);
unused(drivetrain);
// save current manual, cruise, and max velocity parameters
bool result = false;
mavlinkcom::MavLinkParameter manual_velocity_parameter, cruise_velocity_parameter, max_velocity_parameter;
result = mav_vehicle_->getParameter("MPC_VEL_MANUAL").wait(1000, &manual_velocity_parameter);
result = result && mav_vehicle_->getParameter("MPC_XY_CRUISE").wait(1000, &cruise_velocity_parameter);
result = result && mav_vehicle_->getParameter("MPC_XY_VEL_MAX").wait(1000, &max_velocity_parameter);
if (result) {
// set max velocity parameter
mavlinkcom::MavLinkParameter p;
p.name = "MPC_XY_VEL_MAX";
p.value = velocity;
mav_vehicle_->setParameter(p).wait(1000, &result);
if (result) {
const Vector3r& goal_pos = Vector3r(x, y, z);
Vector3r goal_dist_vect;
float goal_dist;
Waiter waiter(getCommandPeriod(), timeout_sec, getCancelToken());
while (!waiter.isComplete()) {
goal_dist_vect = getPosition() - goal_pos;
const Vector3r& goal_normalized = goal_dist_vect.normalized();
goal_dist = goal_dist_vect.dot(goal_normalized);
if (goal_dist > getDistanceAccuracy()) {
moveToPositionInternal(goal_pos, yaw_mode);
//sleep for rest of the cycle
if (!waiter.sleep())
return false;
}
else {
waiter.complete();
}
}
// reset manual, cruise, and max velocity parameters
bool result_temp = false;
mav_vehicle_->setParameter(manual_velocity_parameter).wait(1000, &result);
mav_vehicle_->setParameter(cruise_velocity_parameter).wait(1000, &result_temp);
result = result && result_temp;
mav_vehicle_->setParameter(max_velocity_parameter).wait(1000, &result_temp);
result = result && result_temp;
return result && waiter.isComplete();
}
}
return result;
}
virtual bool hover() override
{
SingleCall lock(this);
bool rc = false;
checkValidVehicle();
mavlinkcom::AsyncResult<bool> result = mav_vehicle_->loiter();
//auto start_time = std::chrono::system_clock::now();
while (!getCancelToken().isCancelled())
{
if (result.wait(100, &rc))
{
break;
}
}
return rc;
}
virtual GeoPoint getHomeGeoPoint() const override
{
updateState();
if (current_state_.home.is_set)
return GeoPoint(current_state_.home.global_pos.lat, current_state_.home.global_pos.lon, current_state_.home.global_pos.alt);
else
return GeoPoint(Utils::nan<double>(), Utils::nan<double>(), Utils::nan<float>());
}
virtual GeoPoint getGpsLocation() const override
{
updateState();
return GeoPoint(current_state_.global_est.pos.lat, current_state_.global_est.pos.lon, current_state_.global_est.pos.alt);
}
virtual void sendTelemetry(float last_interval = -1) override
{
if (logviewer_proxy_ == nullptr || connection_ == nullptr || mav_vehicle_ == nullptr) {
return;
}
mavlinkcom::MavLinkTelemetry data;
connection_->getTelemetry(data);
if (data.messagesReceived == 0) {
if (!hil_message_timer_.started()) {
hil_message_timer_.start();
}
else if (hil_message_timer_.seconds() > messageReceivedTimeout) {
addStatusMessage("not receiving any messages from HIL, please restart your HIL node and try again");
}
}
else {
hil_message_timer_.stop();
}
// listen to the other mavlink connection also
auto mavcon = mav_vehicle_->getConnection();
if (mavcon != connection_) {
mavlinkcom::MavLinkTelemetry gcs;
mavcon->getTelemetry(gcs);
data.handlerMicroseconds += gcs.handlerMicroseconds;
data.messagesHandled += gcs.messagesHandled;
data.messagesReceived += gcs.messagesReceived;
data.messagesSent += gcs.messagesSent;
if (gcs.messagesReceived == 0)
{
if (!gcs_message_timer_.started()) {
gcs_message_timer_.start();
}
else if (gcs_message_timer_.seconds() > messageReceivedTimeout) {
addStatusMessage("not receiving any messages from GCS port, please restart your SITL node and try again");
}
}
else {
gcs_message_timer_.stop();
}
}
data.renderTime = static_cast<int64_t>(last_interval * 1000000);// microseconds
logviewer_proxy_->sendMessage(data);
}
virtual float getCommandPeriod() const override
{
return 1.0f / 50; //1 period of 50hz
}
virtual float getTakeoffZ() const override
{
// pick a number, PX4 doesn't have a fixed limit here, but 3 meters is probably safe
// enough to get out of the backwash turbulence. Negative due to NED coordinate system.
return -3.0f;
}
virtual float getDistanceAccuracy() const override
{
return 0.5f; //measured in simulator by firing commands "MoveToLocation -x 0 -y 0" multiple times and looking at distance traveled
}
protected: //methods
virtual void setControllerGains(uint8_t controllerType, const vector<float>& kp, const vector<float>& ki, const vector<float>& kd) override
{
unused(controllerType);
unused(kp);
unused(ki);
unused(kd);
Utils::log("Not Implemented: setControllerGains", Utils::kLogLevelInfo);
}
virtual void commandMotorPWMs(float front_right_pwm, float front_left_pwm, float rear_right_pwm, float rear_left_pwm) override
{
unused(front_right_pwm);
unused(front_left_pwm);
unused(rear_right_pwm);
unused(rear_left_pwm);
Utils::log("Not Implemented: commandMotorPWMs", Utils::kLogLevelInfo);
}
virtual void commandRollPitchYawZ(float roll, float pitch, float yaw, float z) override
{
if (target_height_ != -z) {
// these PID values were calculated experimentally using AltHoldCommand n MavLinkTest, this provides the best
// control over thrust to achieve minimal over/under shoot in a reasonable amount of time, but it has not
// been tested on a real drone outside jMavSim, so it may need recalibrating...
thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
target_height_ = -z;
}
checkValidVehicle();
auto state = mav_vehicle_->getVehicleState();
float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, thrust);
}
virtual void commandRollPitchYawrateZ(float roll, float pitch, float yaw_rate, float z) override
{
if (target_height_ != -z) {
thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
target_height_ = -z;
}
checkValidVehicle();
auto state = mav_vehicle_->getVehicleState();
float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, thrust);
}
virtual void commandRollPitchYawThrottle(float roll, float pitch, float yaw, float throttle) override
{
checkValidVehicle();
mav_vehicle_->moveByAttitude(roll, pitch, yaw, 0, 0, 0, throttle);
}
virtual void commandRollPitchYawrateThrottle(float roll, float pitch, float yaw_rate, float throttle) override
{
checkValidVehicle();
mav_vehicle_->moveByAttitude(roll, pitch, 0, 0, 0, yaw_rate, throttle);
}
virtual void commandAngleRatesZ(float roll_rate, float pitch_rate, float yaw_rate, float z) override
{
if (target_height_ != -z) {
thrust_controller_.setPoint(-z, .05f, .005f, 0.09f);
target_height_ = -z;
}
checkValidVehicle();
auto state = mav_vehicle_->getVehicleState();
float thrust = 0.21f + thrust_controller_.control(-state.local_est.pos.z);
mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, thrust);
}
virtual void commandAngleRatesThrottle(float roll_rate, float pitch_rate, float yaw_rate, float throttle) override
{
checkValidVehicle();
mav_vehicle_->moveByAttitude(0, 0, 0, roll_rate, pitch_rate, yaw_rate, throttle);
}
virtual void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
{
checkValidVehicle();
float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
mav_vehicle_->moveByLocalVelocity(vx, vy, vz, !yaw_mode.is_rate, yaw);
}
virtual void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override
{
checkValidVehicle();
float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
mav_vehicle_->moveByLocalVelocityWithAltHold(vx, vy, z, !yaw_mode.is_rate, yaw);
}
virtual void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
{
checkValidVehicle();
float yaw = yaw_mode.yaw_or_rate * M_PIf / 180;
mav_vehicle_->moveToLocalPosition(x, y, z, !yaw_mode.is_rate, yaw);
}
//TODO: decouple MultirotorApiBase, VehicalParams and SafetyEval
virtual const MultirotorApiParams& getMultirotorApiParams() const override
{
//defaults are good for PX4 generic quadcopter.
static const MultirotorApiParams vehicle_params_;
return vehicle_params_;
}
virtual void beforeTask() override
{
startOffboardMode();
}
virtual void afterTask() override
{
endOffboardMode();
}
public:
class MavLinkLogViewerLog : public mavlinkcom::MavLinkLog
{
public:
MavLinkLogViewerLog(std::shared_ptr<mavlinkcom::MavLinkNode> proxy) {
proxy_ = proxy;
}
void write(const mavlinkcom::MavLinkMessage& msg, uint64_t timestamp = 0) override {
unused(timestamp);
mavlinkcom::MavLinkMessage copy;
::memcpy(©, &msg, sizeof(mavlinkcom::MavLinkMessage));
proxy_->sendMessage(copy);
}
private:
std::shared_ptr<mavlinkcom::MavLinkNode> proxy_;
};
protected: //methods
virtual void connect()
{
if (!connecting_) {
connecting_ = true;
if (this->connect_thread_.joinable())
{
this->connect_thread_.join();
}
this->connect_thread_ = std::thread(&MavLinkMultirotorApi::connect_thread, this);
}
}
virtual void disconnect() {
addStatusMessage("Disconnecting mavlink vehicle");
connected_ = false;
connecting_ = false;
if (connection_ != nullptr) {
if (is_hil_mode_set_ && mav_vehicle_ != nullptr) {
setNormalMode();
}
connection_->close();
}
if (hil_node_ != nullptr) {
hil_node_->close();
}
if (mav_vehicle_ != nullptr) {
auto c = mav_vehicle_->getConnection();
if (c != nullptr) {
c->stopLoggingSendMessage();
}
mav_vehicle_->close();
mav_vehicle_ = nullptr;
}
if (video_server_ != nullptr)
video_server_->close();
if (logviewer_proxy_ != nullptr) {
logviewer_proxy_->close();
logviewer_proxy_ = nullptr;
}
if (logviewer_out_proxy_ != nullptr) {
logviewer_out_proxy_->close();
logviewer_out_proxy_ = nullptr;
}
if (qgc_proxy_ != nullptr) {
qgc_proxy_->close();
qgc_proxy_ = nullptr;
}
}
void connect_thread()
{
addStatusMessage("Waiting for mavlink vehicle...");
connecting_ = true;
createMavConnection(connection_info_);
if (mav_vehicle_ != nullptr) {
connectToLogViewer();
connectToQGC();
}
connecting_ = false;
connected_ = true;
}
virtual void close()
{
disconnect();
}
void closeAllConnection()
{
close();
}
private: //methods
void openAllConnections()
{
close(); //just in case if connections were open
resetState(); //reset all variables we might have changed during last session
connect();
}
void getMocapPose(Vector3r& position, Quaternionr& orientation) const
{
position.x() = MocapPoseMessage.x; position.y() = MocapPoseMessage.y; position.z() = MocapPoseMessage.z;
orientation.w() = MocapPoseMessage.q[0]; orientation.x() = MocapPoseMessage.q[1];
orientation.y() = MocapPoseMessage.q[2]; orientation.z() = MocapPoseMessage.q[3];
}
//TODO: this method used to send collision to external sim. Do we still need this?
void sendCollision(float normalX, float normalY, float normalZ)
{
checkValidVehicle();
mavlinkcom::MavLinkCollision collision{};
collision.src = 1; //provider of data is MavLink system in id field
collision.id = mav_vehicle_->getLocalSystemId();
collision.action = static_cast<uint8_t>(mavlinkcom::MAV_COLLISION_ACTION::MAV_COLLISION_ACTION_REPORT);
collision.threat_level = static_cast<uint8_t>(mavlinkcom::MAV_COLLISION_THREAT_LEVEL::MAV_COLLISION_THREAT_LEVEL_NONE);
// we are abusing these fields, passing the angle of the object we hit, so that jMAVSim knows how to bounce off.
collision.time_to_minimum_delta = normalX;
collision.altitude_minimum_delta = normalY;
collision.horizontal_minimum_delta = normalZ;
mav_vehicle_->sendMessage(collision);
}
//TODO: do we still need this method?
bool hasVideoRequest()
{
mavlinkcom::MavLinkVideoServer::MavLinkVideoRequest image_req;
return video_server_->hasVideoRequest(image_req);
}
//TODO: do we still need this method?
void sendImage(unsigned char data[], uint32_t length, uint16_t width, uint16_t height)
{
const int MAVLINK_DATA_STREAM_IMG_PNG = 6;
video_server_->sendFrame(data, length, width, height, MAVLINK_DATA_STREAM_IMG_PNG, 0);
}
//put PX4 in normal mode (i.e. non-simulation mode)
void setNormalMode()
{
if (is_hil_mode_set_ && connection_ != nullptr && mav_vehicle_ != nullptr) {
// remove MAV_MODE_FLAG_HIL_ENABLED flag from current mode
std::lock_guard<std::mutex> guard(set_mode_mutex_);
int mode = mav_vehicle_->getVehicleState().mode;
mode &= ~static_cast<int>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
mavlinkcom::MavCmdDoSetMode cmd;
cmd.command = static_cast<uint16_t>(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
cmd.Mode = static_cast<float>(mode);
mav_vehicle_->sendCommand(cmd);
is_hil_mode_set_ = false;
}
}
//put PX4 in simulation mode
void setHILMode()
{
if (!is_simulation_mode_)
throw std::logic_error("Attempt to set device in HIL mode while not in simulation mode");
checkValidVehicle();
// add MAV_MODE_FLAG_HIL_ENABLED flag to current mode
std::lock_guard<std::mutex> guard(set_mode_mutex_);
int mode = mav_vehicle_->getVehicleState().mode;
mode |= static_cast<int>(mavlinkcom::MAV_MODE_FLAG::MAV_MODE_FLAG_HIL_ENABLED);
mavlinkcom::MavCmdDoSetMode cmd;
cmd.command = static_cast<uint16_t>(mavlinkcom::MAV_CMD::MAV_CMD_DO_SET_MODE);
cmd.Mode = static_cast<float>(mode);
mav_vehicle_->sendCommand(cmd);
is_hil_mode_set_ = true;
}
bool startOffboardMode()
{
checkValidVehicle();
try {
mav_vehicle_->requestControl();
}
catch (std::exception& ex) {
ensureSafeMode();
addStatusMessage(std::string("Request control failed: ") + ex.what());
return false;
}
return true;
}
void endOffboardMode()
{
// bugbug: I removed this releaseControl because it makes back-to-back move operations less smooth.
// The side effect of this is that with some drones (e.g. PX4 based) the drone itself will timeout
// when you stop sending move commands and the behavior on timeout is then determined by the drone itself.
// mav_vehicle_->releaseControl();
ensureSafeMode();
}
void ensureSafeMode()
{
if (mav_vehicle_ != nullptr) {
const mavlinkcom::VehicleState& state = mav_vehicle_->getVehicleState();
if (state.controls.landed || !state.controls.armed) {
return;
}
}
}
void checkValidVehicle() {
if (mav_vehicle_ == nullptr || connection_ == nullptr || !connection_->isOpen() || !connected_) {
throw std::logic_error("Cannot perform operation when no vehicle is connected or vehicle is not responding");
}
}
//status update methods should call this first!
void updateState() const
{
StatusLock lock(this);
if (mav_vehicle_ != nullptr) {
int version = mav_vehicle_->getVehicleStateVersion();
if (version != state_version_)
{
current_state_ = mav_vehicle_->getVehicleState();
state_version_ = version;
}
}
}
virtual void normalizeRotorControls()
{
//if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case
//we normalize them to 0 to 1 for PX4
if (!is_controls_0_1_) {
// change -1 to 1 to 0 to 1.
for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f;
}
}
else {
//this applies to PX4 and may work differently on other firmwares.
//We use 0.2 as idle rotors which leaves out range of 0.8
for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f);
}
}
}
bool sendTestMessage(std::shared_ptr<mavlinkcom::MavLinkNode> node) {
try {
// try and send a test message.
mavlinkcom::MavLinkHeartbeat test;
test.autopilot = static_cast<int>(mavlinkcom::MAV_AUTOPILOT::MAV_AUTOPILOT_PX4);
test.type = static_cast<uint8_t>(mavlinkcom::MAV_TYPE::MAV_TYPE_GCS);
test.base_mode = 0;
test.custom_mode = 0;
test.mavlink_version = 3;
node->sendMessage(test);
test.system_status = 0;
return true;
}
catch (std::exception&) {
return false;
}
}
bool connectToLogViewer()
{
//set up logviewer proxy
if (connection_info_.logviewer_ip_address.size() > 0) {
std::shared_ptr<mavlinkcom::MavLinkConnection> connection;
createProxy("LogViewer", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_port, connection_info_.local_host_ip,
logviewer_proxy_, connection);
if (!sendTestMessage(logviewer_proxy_)) {
// error talking to log viewer, so don't keep trying, and close the connection also.
logviewer_proxy_->getConnection()->close();
logviewer_proxy_ = nullptr;
}
std::shared_ptr<mavlinkcom::MavLinkConnection> out_connection;
createProxy("LogViewerOut", connection_info_.logviewer_ip_address, connection_info_.logviewer_ip_sport, connection_info_.local_host_ip,
logviewer_out_proxy_, out_connection);
if (!sendTestMessage(logviewer_out_proxy_)) {
// error talking to log viewer, so don't keep trying, and close the connection also.
logviewer_out_proxy_->getConnection()->close();
logviewer_out_proxy_ = nullptr;
}
else if (mav_vehicle_ != nullptr) {
mav_vehicle_->getConnection()->startLoggingSendMessage(std::make_shared<MavLinkLogViewerLog>(logviewer_out_proxy_));
}
}
return logviewer_proxy_ != nullptr;
}
bool connectToQGC()
{
if (connection_info_.qgc_ip_address.size() > 0) {
std::shared_ptr<mavlinkcom::MavLinkConnection> connection;
createProxy("QGC", connection_info_.qgc_ip_address, connection_info_.qgc_ip_port, connection_info_.local_host_ip, qgc_proxy_, connection);
if (!sendTestMessage(qgc_proxy_)) {
// error talking to QGC, so don't keep trying, and close the connection also.
qgc_proxy_->getConnection()->close();
qgc_proxy_ = nullptr;
}
else {
connection->subscribe([=](std::shared_ptr<mavlinkcom::MavLinkConnection> connection_val, const mavlinkcom::MavLinkMessage& msg) {
unused(connection_val);
processQgcMessages(msg);
});
}
}
return qgc_proxy_ != nullptr;
}
void createProxy(std::string name, std::string ip, int port, string local_host_ip,
std::shared_ptr<mavlinkcom::MavLinkNode>& node, std::shared_ptr<mavlinkcom::MavLinkConnection>& connection)
{
if (connection_ == nullptr)
throw std::domain_error("MavLinkMultirotorApi requires connection object to be set before createProxy call");
connection = mavlinkcom::MavLinkConnection::connectRemoteUdp("Proxy to: " + name + " at " + ip + ":" + std::to_string(port), local_host_ip, ip, port);
// it is ok to reuse the simulator sysid and compid here because this node is only used to send a few messages directly to this endpoint
// and all other messages are funneled through from PX4 via the Join method below.
node = std::make_shared<mavlinkcom::MavLinkNode>(connection_info_.sim_sysid, connection_info_.sim_compid);
node->connect(connection);
// now join the main connection to this one, this causes all PX4 messages to be sent to the proxy and all messages from the proxy will be
// send directly to the PX4 (using whatever sysid/compid comes from that remote node).
connection_->join(connection);
auto mavcon = mav_vehicle_->getConnection();
if (mavcon != connection_) {
mavcon->join(connection);
}
}
static std::string findPX4()
{
auto result = mavlinkcom::MavLinkConnection::findSerialPorts(0, 0);