CoCalc -- GCS_MAVLink

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/Rover/GCS_MAVLink_Rover.cpp
Views: ¹⁷⁹⁸
1
#include "Rover.h"
2

3
#include "GCS_MAVLink_Rover.h"
4

5
#include <AP_RPM/AP_RPM_config.h>
6
#include <AP_RangeFinder/AP_RangeFinder_Backend.h>
7
#include <AP_EFI/AP_EFI_config.h>
8
#include <AC_Avoidance/AP_OADatabase.h>
9

10
MAV_TYPE GCS_Rover::frame_type() const
11
{
12
    if (rover.is_boat()) {
13
        return MAV_TYPE_SURFACE_BOAT;
14
    }
15
    return MAV_TYPE_GROUND_ROVER;
16
}
17

18
MAV_MODE GCS_MAVLINK_Rover::base_mode() const
19
{
20
    uint8_t _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
21

22
    // work out the base_mode. This value is not very useful
23
    // for APM, but we calculate it as best we can so a generic
24
    // MAVLink enabled ground station can work out something about
25
    // what the MAV is up to. The actual bit values are highly
26
    // ambiguous for most of the APM flight modes. In practice, you
27
    // only get useful information from the custom_mode, which maps to
28
    // the APM flight mode and has a well defined meaning in the
29
    // ArduPlane documentation
30
    if (rover.control_mode->has_manual_input()) {
31
        _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
32
    }
33

34
    if (rover.control_mode->is_autopilot_mode()) {
35
        _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
36
    }
37

38
    if (rover.g2.stick_mixing > 0 && rover.control_mode != &rover.mode_initializing) {
39
        // all modes except INITIALISING have some form of manual
40
        // override if stick mixing is enabled
41
        _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
42
    }
43

44
    // we are armed if we are not initialising
45
    if (rover.control_mode != &rover.mode_initializing && rover.arming.is_armed()) {
46
        _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
47
    }
48

49
    // indicate we have set a custom mode
50
    _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
51

52
    return (MAV_MODE)_base_mode;
53
}
54

55
uint32_t GCS_Rover::custom_mode() const
56
{
57
    return (uint32_t)rover.control_mode->mode_number();
58
}
59

60
MAV_STATE GCS_MAVLINK_Rover::vehicle_system_status() const
61
{
62
    if ((rover.failsafe.triggered != 0) || rover.failsafe.ekf) {
63
        return MAV_STATE_CRITICAL;
64
    }
65
    if (rover.control_mode == &rover.mode_initializing) {
66
        return MAV_STATE_CALIBRATING;
67
    }
68
    if (rover.control_mode == &rover.mode_hold) {
69
        return MAV_STATE_STANDBY;
70
    }
71

72
    return MAV_STATE_ACTIVE;
73
}
74

75
void GCS_MAVLINK_Rover::send_position_target_global_int()
76
{
77
    Location target;
78
    if (!rover.control_mode->get_desired_location(target)) {
79
        return;
80
    }
81
    static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000;
82
    static constexpr uint16_t TYPE_MASK = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
83
                                          POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
84
                                          POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE;
85
    mavlink_msg_position_target_global_int_send(
86
        chan,
87
        AP_HAL::millis(), // time_boot_ms
88
        MAV_FRAME_GLOBAL, // targets are always global altitude
89
        TYPE_MASK, // ignore everything except the x/y/z components
90
        target.lat, // latitude as 1e7
91
        target.lng, // longitude as 1e7
92
        target.alt * 0.01f, // altitude is sent as a float
93
        0.0f, // vx
94
        0.0f, // vy
95
        0.0f, // vz
96
        0.0f, // afx
97
        0.0f, // afy
98
        0.0f, // afz
99
        0.0f, // yaw
100
        0.0f); // yaw_rate
101
}
102

103
void GCS_MAVLINK_Rover::send_nav_controller_output() const
104
{
105
    if (!rover.control_mode->is_autopilot_mode()) {
106
        return;
107
    }
108

109
    const Mode *control_mode = rover.control_mode;
110

111
    mavlink_msg_nav_controller_output_send(
112
        chan,
113
        0,  // roll
114
        degrees(rover.g2.attitude_control.get_desired_pitch()),
115
        control_mode->nav_bearing(),
116
        control_mode->wp_bearing(),
117
        MIN(control_mode->get_distance_to_destination(), UINT16_MAX),
118
        0,
119
        control_mode->speed_error(),
120
        control_mode->crosstrack_error());
121
}
122

123
void GCS_MAVLINK_Rover::send_servo_out()
124
{
125
    float motor1, motor3;
126
    if (rover.g2.motors.have_skid_steering()) {
127
        motor1 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttleLeft) * 0.001f);
128
        motor3 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttleRight) * 0.001f);
129
    } else {
130
        motor1 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_steering) / 4500.0f);
131
        motor3 = 10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * 0.01f);
132
    }
133
    mavlink_msg_rc_channels_scaled_send(
134
        chan,
135
        millis(),
136
        0,  // port 0
137
        motor1,
138
        0,
139
        motor3,
140
        0,
141
        0,
142
        0,
143
        0,
144
        0,
145
#if AP_RSSI_ENABLED
146
        receiver_rssi()
147
#else
148
        255
149
#endif
150
        );
151
}
152

153
int16_t GCS_MAVLINK_Rover::vfr_hud_throttle() const
154
{
155
    return rover.g2.motors.get_throttle();
156
}
157

158
#if AP_RANGEFINDER_ENABLED
159
void GCS_MAVLINK_Rover::send_rangefinder() const
160
{
161
    float distance = 0;
162
    float voltage = 0;
163
    bool got_one = false;
164

165
    // report smaller distance of all rangefinders
166
    for (uint8_t i=0; i<rover.rangefinder.num_sensors(); i++) {
167
        AP_RangeFinder_Backend *s = rover.rangefinder.get_backend(i);
168
        if (s == nullptr) {
169
            continue;
170
        }
171
        if (!got_one ||
172
            s->distance() < distance) {
173
            distance = s->distance();
174
            voltage = s->voltage_mv();
175
            got_one = true;
176
        }
177
    }
178
    if (!got_one) {
179
        // no relevant data found
180
        return;
181
    }
182

183
    mavlink_msg_rangefinder_send(
184
        chan,
185
        distance,
186
        voltage);
187
}
188

189
void GCS_MAVLINK_Rover::send_water_depth()
190
{
191
    if (!HAVE_PAYLOAD_SPACE(chan, WATER_DEPTH)) {
192
        return;
193
    }
194

195
    // only send for boats:
196
    if (!rover.is_boat()) {
197
        return;
198
    }
199

200
    RangeFinder *rangefinder = RangeFinder::get_singleton();
201

202
    if (rangefinder == nullptr) {
203
        return;
204
    }
205

206
    // depth can only be measured by a downward-facing rangefinder:
207
    if (!rangefinder->has_orientation(ROTATION_PITCH_270)) {
208
        return;
209
    }
210

211
    // get position
212
    const AP_AHRS &ahrs = AP::ahrs();
213
    Location loc;
214
    IGNORE_RETURN(ahrs.get_location(loc));
215

216
    const auto num_sensors = rangefinder->num_sensors();
217
    for (uint8_t i=0; i<num_sensors; i++) {
218
        last_WATER_DEPTH_index += 1;
219
        if (last_WATER_DEPTH_index >= num_sensors) {
220
            last_WATER_DEPTH_index = 0;
221
        }
222

223
        const AP_RangeFinder_Backend *s = rangefinder->get_backend(last_WATER_DEPTH_index);
224

225
        if (s == nullptr || s->orientation() != ROTATION_PITCH_270 || !s->has_data()) {
226
            continue;
227
        }
228

229
        // get temperature
230
        float temp_C;
231
        if (!s->get_temp(temp_C)) {
232
            temp_C = 0.0f;
233
        }
234

235
        const bool sensor_healthy = (s->status() == RangeFinder::Status::Good);
236

237
        mavlink_msg_water_depth_send(
238
            chan,
239
            AP_HAL::millis(),   // time since system boot TODO: take time of measurement
240
            last_WATER_DEPTH_index, // rangefinder instance
241
            sensor_healthy,     // sensor healthy
242
            loc.lat,            // latitude of vehicle
243
            loc.lng,            // longitude of vehicle
244
            loc.alt * 0.01f,    // altitude of vehicle (MSL)
245
            ahrs.get_roll(),    // roll in radians
246
            ahrs.get_pitch(),   // pitch in radians
247
            ahrs.get_yaw(),     // yaw in radians
248
            s->distance(),    // distance in meters
249
            temp_C);            // temperature in degC
250

251
        break;  // only send one WATER_DEPTH message per loop
252
    }
253

254
}
255
#endif  // AP_RANGEFINDER_ENABLED
256

257
/*
258
  send PID tuning message
259
 */
260
void GCS_MAVLINK_Rover::send_pid_tuning()
261
{
262
    Parameters &g = rover.g;
263
    ParametersG2 &g2 = rover.g2;
264

265
    const AP_PIDInfo *pid_info;
266

267
    // steering PID
268
    if (g.gcs_pid_mask & 1) {
269
        pid_info = &g2.attitude_control.get_steering_rate_pid().get_pid_info();
270
        mavlink_msg_pid_tuning_send(chan, PID_TUNING_STEER,
271
                                    degrees(pid_info->target),
272
                                    degrees(pid_info->actual),
273
                                    pid_info->FF,
274
                                    pid_info->P,
275
                                    pid_info->I,
276
                                    pid_info->D,
277
                                    pid_info->slew_rate,
278
                                    pid_info->Dmod);
279
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
280
            return;
281
        }
282
    }
283

284
    // speed to throttle PID
285
    if (g.gcs_pid_mask & 2) {
286
        pid_info = &g2.attitude_control.get_throttle_speed_pid_info();
287
        mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
288
                                    pid_info->target,
289
                                    pid_info->actual,
290
                                    pid_info->FF,
291
                                    pid_info->P,
292
                                    pid_info->I,
293
                                    pid_info->D,
294
                                    pid_info->slew_rate,
295
                                    pid_info->Dmod);
296
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
297
            return;
298
        }
299
    }
300

301
    // pitch to throttle pid
302
    if (g.gcs_pid_mask & 4) {
303
        pid_info = &g2.attitude_control.get_pitch_to_throttle_pid().get_pid_info();
304
        mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
305
                                    degrees(pid_info->target),
306
                                    degrees(pid_info->actual),
307
                                    pid_info->FF,
308
                                    pid_info->P,
309
                                    pid_info->I,
310
                                    pid_info->D,
311
                                    pid_info->slew_rate,
312
                                    pid_info->Dmod);
313
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
314
            return;
315
        }
316
    }
317

318
    // left wheel rate control pid
319
    if (g.gcs_pid_mask & 8) {
320
        pid_info = &g2.wheel_rate_control.get_pid(0).get_pid_info();
321
        mavlink_msg_pid_tuning_send(chan, 7,
322
                                    pid_info->target,
323
                                    pid_info->actual,
324
                                    pid_info->FF,
325
                                    pid_info->P,
326
                                    pid_info->I,
327
                                    pid_info->D,
328
                                    pid_info->slew_rate,
329
                                    pid_info->Dmod);
330
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
331
            return;
332
        }
333
    }
334

335
    // right wheel rate control pid
336
    if (g.gcs_pid_mask & 16) {
337
        pid_info = &g2.wheel_rate_control.get_pid(1).get_pid_info();
338
        mavlink_msg_pid_tuning_send(chan, 8,
339
                                    pid_info->target,
340
                                    pid_info->actual,
341
                                    pid_info->FF,
342
                                    pid_info->P,
343
                                    pid_info->I,
344
                                    pid_info->D,
345
                                    pid_info->slew_rate,
346
                                    pid_info->Dmod);
347
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
348
            return;
349
        }
350
    }
351

352
    // sailboat heel to mainsail pid
353
    if (g.gcs_pid_mask & 32) {
354
        pid_info = &g2.attitude_control.get_sailboat_heel_pid().get_pid_info();
355
        mavlink_msg_pid_tuning_send(chan, 9,
356
                                    pid_info->target,
357
                                    pid_info->actual,
358
                                    pid_info->FF,
359
                                    pid_info->P,
360
                                    pid_info->I,
361
                                    pid_info->D,
362
                                    pid_info->slew_rate,
363
                                    pid_info->Dmod);
364
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
365
            return;
366
        }
367
    }
368

369
    // Position Controller Velocity North PID
370
    if (g.gcs_pid_mask & 64) {
371
        pid_info = &g2.pos_control.get_vel_pid().get_pid_info_x();
372
        mavlink_msg_pid_tuning_send(chan, 10,
373
                                    pid_info->target,
374
                                    pid_info->actual,
375
                                    pid_info->FF,
376
                                    pid_info->P,
377
                                    pid_info->I,
378
                                    pid_info->D,
379
                                    pid_info->slew_rate,
380
                                    pid_info->Dmod);
381
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
382
            return;
383
        }
384
    }
385

386
    // Position Controller Velocity East PID
387
    if (g.gcs_pid_mask & 128) {
388
        pid_info = &g2.pos_control.get_vel_pid().get_pid_info_y();
389
        mavlink_msg_pid_tuning_send(chan, 11,
390
                                    pid_info->target,
391
                                    pid_info->actual,
392
                                    pid_info->FF,
393
                                    pid_info->P,
394
                                    pid_info->I,
395
                                    pid_info->D,
396
                                    pid_info->slew_rate,
397
                                    pid_info->Dmod);
398
        if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
399
            return;
400
        }
401
    }
402
}
403

404
void Rover::send_wheel_encoder_distance(const mavlink_channel_t chan)
405
{
406
    // send wheel encoder data using wheel_distance message
407
    if (g2.wheel_encoder.num_sensors() > 0) {
408
        double distances[MAVLINK_MSG_WHEEL_DISTANCE_FIELD_DISTANCE_LEN] {};
409
        for (uint8_t i = 0; i < g2.wheel_encoder.num_sensors(); i++) {
410
            distances[i] = wheel_encoder_last_distance_m[i];
411
        }
412
        mavlink_msg_wheel_distance_send(chan, 1000UL * AP_HAL::millis(), g2.wheel_encoder.num_sensors(), distances);
413
    }
414
}
415

416
uint8_t GCS_MAVLINK_Rover::sysid_my_gcs() const
417
{
418
    return rover.g.sysid_my_gcs;
419
}
420

421
bool GCS_MAVLINK_Rover::sysid_enforce() const
422
{
423
    return rover.g2.sysid_enforce;
424
}
425

426
uint32_t GCS_MAVLINK_Rover::telem_delay() const
427
{
428
    return static_cast<uint32_t>(rover.g.telem_delay);
429
}
430

431
bool GCS_Rover::vehicle_initialised() const
432
{
433
    return rover.control_mode != &rover.mode_initializing;
434
}
435

436
// try to send a message, return false if it won't fit in the serial tx buffer
437
bool GCS_MAVLINK_Rover::try_send_message(enum ap_message id)
438
{
439
    switch (id) {
440

441
    case MSG_SERVO_OUT:
442
        CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
443
        send_servo_out();
444
        break;
445

446
    case MSG_WHEEL_DISTANCE:
447
        CHECK_PAYLOAD_SIZE(WHEEL_DISTANCE);
448
        rover.send_wheel_encoder_distance(chan);
449
        break;
450

451
    case MSG_WIND:
452
        CHECK_PAYLOAD_SIZE(WIND);
453
        rover.g2.windvane.send_wind(chan);
454
        break;
455

456
#if AP_OADATABASE_ENABLED
457
    case MSG_ADSB_VEHICLE: {
458
        AP_OADatabase *oadb = AP::oadatabase();
459
        if (oadb != nullptr) {
460
            CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
461
            uint16_t interval_ms = 0;
462
            if (get_ap_message_interval(id, interval_ms)) {
463
                oadb->send_adsb_vehicle(chan, interval_ms);
464
            }
465
        }
466
        break;
467
    }
468
#endif
469

470
#if AP_RANGEFINDER_ENABLED
471
    case MSG_WATER_DEPTH:
472
        CHECK_PAYLOAD_SIZE(WATER_DEPTH);
473
        send_water_depth();
474
        break;
475
#endif  // AP_RANGEFINDER_ENABLED
476

477
    default:
478
        return GCS_MAVLINK::try_send_message(id);
479
    }
480
    return true;
481
}
482

483
void GCS_MAVLINK_Rover::packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg)
484
{
485
#if AP_FOLLOW_ENABLED
486
    // pass message to follow library
487
    rover.g2.follow.handle_msg(msg);
488
#endif
489
    GCS_MAVLINK::packetReceived(status, msg);
490
}
491

492
/*
493
  default stream rates to 1Hz
494
 */
495
const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
496
    // @Param: RAW_SENS
497
    // @DisplayName: Raw sensor stream rate
498
    // @Description: MAVLink Stream rate of RAW_IMU, SCALED_IMU2, SCALED_IMU3, SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3 and AIRSPEED
499
    // @Units: Hz
500
    // @Range: 0 50
501
    // @Increment: 1
502
    // @RebootRequired: True
503
    // @User: Advanced
504
    AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0],  1),
505

506
    // @Param: EXT_STAT
507
    // @DisplayName: Extended status stream rate
508
    // @Description: MAVLink Stream rate of SYS_STATUS, POWER_STATUS, MCU_STATUS, MEMINFO, CURRENT_WAYPOINT, GPS_RAW_INT, GPS_RTK (if available), GPS2_RAW_INT (if available), GPS2_RTK (if available), NAV_CONTROLLER_OUTPUT, FENCE_STATUS, and GLOBAL_TARGET_POS_INT
509
    // @Units: Hz
510
    // @Range: 0 50
511
    // @Increment: 1
512
    // @RebootRequired: True
513
    // @User: Advanced
514
    AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1],  1),
515

516
    // @Param: RC_CHAN
517
    // @DisplayName: RC Channel stream rate
518
    // @Description: MAVLink Stream rate of SERVO_OUTPUT_RAW and RC_CHANNELS
519
    // @Units: Hz
520
    // @Range: 0 50
521
    // @Increment: 1
522
    // @RebootRequired: True
523
    // @User: Advanced
524
    AP_GROUPINFO("RC_CHAN",  2, GCS_MAVLINK_Parameters, streamRates[2],  1),
525

526
    // @Param: RAW_CTRL
527
    // @DisplayName: Raw Control stream rate
528
    // @Description: MAVLink Raw Control stream rate of SERVO_OUT
529
    // @Units: Hz
530
    // @Range: 0 50
531
    // @Increment: 1
532
    // @RebootRequired: True
533
    // @User: Advanced
534
    AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3],  1),
535

536
    // @Param: POSITION
537
    // @DisplayName: Position stream rate
538
    // @Description: MAVLink Stream rate of GLOBAL_POSITION_INT and LOCAL_POSITION_NED
539
    // @Units: Hz
540
    // @Range: 0 50
541
    // @Increment: 1
542
    // @RebootRequired: True
543
    // @User: Advanced
544
    AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4],  1),
545

546
    // @Param: EXTRA1
547
    // @DisplayName: Extra data type 1 stream rate to ground station
548
    // @Description: MAVLink Stream rate of ATTITUDE, SIMSTATE (SIM only), AHRS2 and PID_TUNING
549
    // @Units: Hz
550
    // @Range: 0 50
551
    // @Increment: 1
552
    // @RebootRequired: True
553
    // @User: Advanced
554
    AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK_Parameters, streamRates[5],  1),
555

556
    // @Param: EXTRA2
557
    // @DisplayName: Extra data type 2 stream rate
558
    // @Description: MAVLink Stream rate of VFR_HUD
559
    // @Units: Hz
560
    // @Range: 0 50
561
    // @Increment: 1
562
    // @RebootRequired: True
563
    // @User: Advanced
564
    AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK_Parameters, streamRates[6],  1),
565

566
    // @Param: EXTRA3
567
    // @DisplayName: Extra data type 3 stream rate
568
    // @Description: MAVLink Stream rate of AHRS, SYSTEM_TIME, WIND, RANGEFINDER, DISTANCE_SENSOR, BATTERY2, BATTERY_STATUS, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION, RPM, ESC TELEMETRY, and WHEEL_DISTANCE
569
    // @Units: Hz
570
    // @Range: 0 50
571
    // @Increment: 1
572
    // @RebootRequired: True
573
    // @User: Advanced
574
    AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK_Parameters, streamRates[7],  1),
575

576
    // @Param: PARAMS
577
    // @DisplayName: Parameter stream rate
578
    // @Description: MAVLink Stream rate of PARAM_VALUE
579
    // @Units: Hz
580
    // @Range: 0 50
581
    // @Increment: 1
582
    // @RebootRequired: True
583
    // @User: Advanced
584
    AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK_Parameters, streamRates[8],  10),
585

586
    // @Param: ADSB
587
    // @DisplayName: ADSB stream rate
588
    // @Description: MAVLink ADSB (AIS) stream rate
589
    // @Units: Hz
590
    // @Range: 0 50
591
    // @Increment: 1
592
    // @RebootRequired: True
593
    // @User: Advanced
594
    AP_GROUPINFO("ADSB",   9, GCS_MAVLINK_Parameters, streamRates[9],  0),
595

596
    AP_GROUPEND
597
};
598

599
static const ap_message STREAM_RAW_SENSORS_msgs[] = {
600
    MSG_RAW_IMU,
601
    MSG_SCALED_IMU2,
602
    MSG_SCALED_IMU3,
603
    MSG_SCALED_PRESSURE,
604
    MSG_SCALED_PRESSURE2,
605
    MSG_SCALED_PRESSURE3,
606
#if AP_AIRSPEED_ENABLED
607
    MSG_AIRSPEED,
608
#endif
609
};
610
static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
611
    MSG_SYS_STATUS,
612
    MSG_POWER_STATUS,
613
#if HAL_WITH_MCU_MONITORING
614
    MSG_MCU_STATUS,
615
#endif
616
    MSG_MEMINFO,
617
    MSG_CURRENT_WAYPOINT,
618
#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED
619
    MSG_GPS_RAW,
620
#endif
621
#if AP_GPS_GPS_RTK_SENDING_ENABLED
622
    MSG_GPS_RTK,
623
#endif
624
#if AP_GPS_GPS2_RAW_SENDING_ENABLED
625
    MSG_GPS2_RAW,
626
#endif
627
#if AP_GPS_GPS2_RTK_SENDING_ENABLED
628
    MSG_GPS2_RTK,
629
#endif
630
    MSG_NAV_CONTROLLER_OUTPUT,
631
#if AP_FENCE_ENABLED
632
    MSG_FENCE_STATUS,
633
#endif
634
    MSG_POSITION_TARGET_GLOBAL_INT,
635
};
636
static const ap_message STREAM_POSITION_msgs[] = {
637
    MSG_LOCATION,
638
    MSG_LOCAL_POSITION
639
};
640
static const ap_message STREAM_RAW_CONTROLLER_msgs[] = {
641
    MSG_SERVO_OUT,
642
};
643
static const ap_message STREAM_RC_CHANNELS_msgs[] = {
644
    MSG_SERVO_OUTPUT_RAW,
645
    MSG_RC_CHANNELS,
646
#if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED
647
    MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
648
#endif
649
};
650
static const ap_message STREAM_EXTRA1_msgs[] = {
651
    MSG_ATTITUDE,
652
#if AP_SIM_ENABLED
653
    MSG_SIMSTATE,
654
#endif
655
    MSG_AHRS2,
656
    MSG_PID_TUNING,
657
};
658
static const ap_message STREAM_EXTRA2_msgs[] = {
659
    MSG_VFR_HUD
660
};
661
static const ap_message STREAM_EXTRA3_msgs[] = {
662
    MSG_AHRS,
663
    MSG_WIND,
664
#if AP_RANGEFINDER_ENABLED
665
    MSG_RANGEFINDER,
666
    MSG_WATER_DEPTH,
667
#endif
668
    MSG_DISTANCE_SENSOR,
669
    MSG_SYSTEM_TIME,
670
#if AP_BATTERY_ENABLED
671
    MSG_BATTERY_STATUS,
672
#endif
673
#if HAL_MOUNT_ENABLED
674
    MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
675
#endif
676
#if AP_OPTICALFLOW_ENABLED
677
    MSG_OPTICAL_FLOW,
678
#endif
679
#if COMPASS_CAL_ENABLED
680
    MSG_MAG_CAL_REPORT,
681
    MSG_MAG_CAL_PROGRESS,
682
#endif
683
    MSG_EKF_STATUS_REPORT,
684
    MSG_VIBRATION,
685
#if AP_RPM_ENABLED
686
    MSG_RPM,
687
#endif
688
    MSG_WHEEL_DISTANCE,
689
#if HAL_WITH_ESC_TELEM
690
    MSG_ESC_TELEMETRY,
691
#endif
692
#if HAL_EFI_ENABLED
693
    MSG_EFI_STATUS,
694
#endif
695
};
696
static const ap_message STREAM_PARAMS_msgs[] = {
697
    MSG_NEXT_PARAM,
698
    MSG_AVAILABLE_MODES
699
};
700
static const ap_message STREAM_ADSB_msgs[] = {
701
    MSG_ADSB_VEHICLE,
702
#if AP_AIS_ENABLED
703
    MSG_AIS_VESSEL,
704
#endif
705
};
706

707
const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
708
    MAV_STREAM_ENTRY(STREAM_RAW_SENSORS),
709
    MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS),
710
    MAV_STREAM_ENTRY(STREAM_POSITION),
711
    MAV_STREAM_ENTRY(STREAM_RAW_CONTROLLER),
712
    MAV_STREAM_ENTRY(STREAM_RC_CHANNELS),
713
    MAV_STREAM_ENTRY(STREAM_EXTRA1),
714
    MAV_STREAM_ENTRY(STREAM_EXTRA2),
715
    MAV_STREAM_ENTRY(STREAM_EXTRA3),
716
    MAV_STREAM_ENTRY(STREAM_ADSB),
717
    MAV_STREAM_ENTRY(STREAM_PARAMS),
718
    MAV_STREAM_TERMINATOR // must have this at end of stream_entries
719
};
720

721
bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd)
722
{
723
    if (!rover.control_mode->in_guided_mode()) {
724
        // only accept position updates when in GUIDED mode
725
        return false;
726
    }
727

728
    // make any new wp uploaded instant (in case we are already in Guided mode)
729
    return rover.mode_guided.set_desired_location(cmd.content.location);
730
}
731

732
MAV_RESULT GCS_MAVLINK_Rover::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
733
{
734
    if (packet.y == 1) {
735
        if (rover.g2.windvane.start_direction_calibration()) {
736
            return MAV_RESULT_ACCEPTED;
737
        } else {
738
            return MAV_RESULT_FAILED;
739
        }
740
    } else if (packet.y == 2) {
741
        if (rover.g2.windvane.start_speed_calibration()) {
742
            return MAV_RESULT_ACCEPTED;
743
        } else {
744
            return MAV_RESULT_FAILED;
745
        }
746
    }
747

748
    return GCS_MAVLINK::_handle_command_preflight_calibration(packet, msg);
749
}
750

751
MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
752
{
753
    switch (packet.command) {
754

755
    case MAV_CMD_DO_CHANGE_SPEED:
756
        // param1 : unused
757
        // param2 : new speed in m/s
758
        if (!rover.control_mode->set_desired_speed(packet.param2)) {
759
            return MAV_RESULT_FAILED;
760
        }
761
        return MAV_RESULT_ACCEPTED;
762

763
    case MAV_CMD_DO_REPOSITION:
764
        return handle_command_int_do_reposition(packet);
765

766
    case MAV_CMD_DO_SET_REVERSE:
767
        // param1 : Direction (0=Forward, 1=Reverse)
768
        rover.control_mode->set_reversed(is_equal(packet.param1,1.0f));
769
        return MAV_RESULT_ACCEPTED;
770

771
    case MAV_CMD_NAV_RETURN_TO_LAUNCH:
772
        if (rover.set_mode(rover.mode_rtl, ModeReason::GCS_COMMAND)) {
773
            return MAV_RESULT_ACCEPTED;
774
        }
775
        return MAV_RESULT_FAILED;
776

777
    case MAV_CMD_DO_MOTOR_TEST:
778
        // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
779
        // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
780
        // param3 : throttle (range depends upon param2)
781
        // param4 : timeout (in seconds)
782
        return rover.mavlink_motor_test_start(*this,
783
                                              (AP_MotorsUGV::motor_test_order)packet.param1,
784
                                              static_cast<uint8_t>(packet.param2),
785
                                              static_cast<int16_t>(packet.param3),
786
                                              packet.param4);
787

788
    case MAV_CMD_MISSION_START:
789
        if (!is_zero(packet.param1) || !is_zero(packet.param2)) {
790
            // first-item/last item not supported
791
            return MAV_RESULT_DENIED;
792
        }
793
        if (rover.set_mode(rover.mode_auto, ModeReason::GCS_COMMAND)) {
794
            return MAV_RESULT_ACCEPTED;
795
        }
796
        return MAV_RESULT_FAILED;
797

798
#if AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED
799
    case MAV_CMD_NAV_SET_YAW_SPEED:
800
        send_received_message_deprecation_warning("MAV_CMD_NAV_SET_YAW_SPEED");
801
        return handle_command_nav_set_yaw_speed(packet, msg);
802
#endif
803

804
    default:
805
        return GCS_MAVLINK::handle_command_int_packet(packet, msg);
806
    }
807
}
808

809
#if AP_MAVLINK_MAV_CMD_NAV_SET_YAW_SPEED_ENABLED
810
MAV_RESULT GCS_MAVLINK_Rover::handle_command_nav_set_yaw_speed(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
811
{
812
        // param1 : yaw angle (may be absolute or relative)
813
        // param2 : Speed - in metres/second
814
        // param3 : 0 = param1 is absolute, 1 = param1 is relative
815

816
        // exit if vehicle is not in Guided mode
817
        if (!rover.control_mode->in_guided_mode()) {
818
            return MAV_RESULT_FAILED;
819
        }
820

821
        // get final angle, 1 = Relative, 0 = Absolute
822
        if (packet.param3 > 0) {
823
            // relative angle
824
            rover.mode_guided.set_desired_heading_delta_and_speed(packet.param1 * 100.0f, packet.param2);
825
        } else {
826
            // absolute angle
827
            rover.mode_guided.set_desired_heading_and_speed(packet.param1 * 100.0f, packet.param2);
828
        }
829
        return MAV_RESULT_ACCEPTED;
830
}
831
#endif
832

833
MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
834
{
835
    const bool change_modes = ((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) == MAV_DO_REPOSITION_FLAGS_CHANGE_MODE;
836
    if (!rover.control_mode->in_guided_mode() && !change_modes) {
837
        return MAV_RESULT_DENIED;
838
    }
839

840
    // sanity check location
841
    if (!check_latlng(packet.x, packet.y)) {
842
        return MAV_RESULT_DENIED;
843
    }
844
    if (packet.x == 0 && packet.y == 0) {
845
        return MAV_RESULT_DENIED;
846
    }
847

848
    Location requested_location {};
849
    if (!location_from_command_t(packet, requested_location)) {
850
        return MAV_RESULT_DENIED;
851
    }
852

853
    if (!rover.control_mode->in_guided_mode()) {
854
        if (!rover.set_mode(Mode::Number::GUIDED, ModeReason::GCS_COMMAND)) {
855
            return MAV_RESULT_FAILED;
856
        }
857
    }
858

859
    if (is_positive(packet.param1)) {
860
        if (!rover.control_mode->set_desired_speed(packet.param1)) {
861
            return MAV_RESULT_FAILED;
862
        }
863
    }
864

865
    // set the destination
866
    if (!rover.mode_guided.set_desired_location(requested_location)) {
867
        return MAV_RESULT_FAILED;
868
    }
869

870
    return MAV_RESULT_ACCEPTED;
871
}
872

873
void GCS_MAVLINK_Rover::handle_message(const mavlink_message_t &msg)
874
{
875
    switch (msg.msgid) {
876

877
    case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
878
        handle_set_attitude_target(msg);
879
        break;
880

881
    case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
882
        handle_set_position_target_local_ned(msg);
883
        break;
884

885
    case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
886
        handle_set_position_target_global_int(msg);
887
        break;
888

889
    default:
890
        GCS_MAVLINK::handle_message(msg);
891
        break;
892
    }
893
}
894

895
void GCS_MAVLINK_Rover::handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow)
896
{
897
    manual_override(rover.channel_steer, packet.y, 1000, 2000, tnow);
898
    manual_override(rover.channel_throttle, packet.z, 1000, 2000, tnow);
899
}
900

901
void GCS_MAVLINK_Rover::handle_set_attitude_target(const mavlink_message_t &msg)
902
{
903
    // decode packet
904
    mavlink_set_attitude_target_t packet;
905
    mavlink_msg_set_attitude_target_decode(&msg, &packet);
906

907
    // exit if vehicle is not in Guided mode
908
    if (!rover.control_mode->in_guided_mode()) {
909
        return;
910
    }
911

912
    // ensure type_mask specifies to use thrust
913
    if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_THROTTLE_IGNORE) != 0) {
914
        return;
915
    }
916

917
    // convert thrust to ground speed
918
    packet.thrust = constrain_float(packet.thrust, -1.0f, 1.0f);
919
    const float target_speed = rover.control_mode->get_speed_default() * packet.thrust;
920

921
    // if the body_yaw_rate field is ignored, convert quaternion to heading
922
    if ((packet.type_mask & MAVLINK_SET_ATT_TYPE_MASK_YAW_RATE_IGNORE) != 0) {
923
        // convert quaternion to heading
924
        float target_heading_cd = degrees(Quaternion(packet.q[0], packet.q[1], packet.q[2], packet.q[3]).get_euler_yaw()) * 100.0f;
925
        rover.mode_guided.set_desired_heading_and_speed(target_heading_cd, target_speed);
926
    } else {
927
        // use body_yaw_rate field
928
        rover.mode_guided.set_desired_turn_rate_and_speed((RAD_TO_DEG * packet.body_yaw_rate) * 100.0f, target_speed);
929
    }
930
}
931

932
void GCS_MAVLINK_Rover::handle_set_position_target_local_ned(const mavlink_message_t &msg)
933
{
934
    // decode packet
935
    mavlink_set_position_target_local_ned_t packet;
936
    mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
937

938
    // exit if vehicle is not in Guided mode
939
    if (!rover.control_mode->in_guided_mode()) {
940
        return;
941
    }
942

943
    // need ekf origin
944
    Location ekf_origin;
945
    if (!rover.ahrs.get_origin(ekf_origin)) {
946
        return;
947
    }
948

949
    // check for supported coordinate frames
950
    switch (packet.coordinate_frame) {
951
    case MAV_FRAME_LOCAL_NED:
952
    case MAV_FRAME_LOCAL_OFFSET_NED:
953
    case MAV_FRAME_BODY_NED:
954
    case MAV_FRAME_BODY_OFFSET_NED:
955
        break;
956

957
    default:
958
        return;
959
    }
960

961
    bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
962
    bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
963
    bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
964
    bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
965
    bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
966

967
    // prepare target position
968
    Location target_loc = rover.current_loc;
969
    if (!pos_ignore) {
970
        switch (packet.coordinate_frame) {
971
        case MAV_FRAME_BODY_NED:
972
        case MAV_FRAME_BODY_OFFSET_NED: {
973
            // rotate from body-frame to NE frame
974
            const float ne_x = packet.x * rover.ahrs.cos_yaw() - packet.y * rover.ahrs.sin_yaw();
975
            const float ne_y = packet.x * rover.ahrs.sin_yaw() + packet.y * rover.ahrs.cos_yaw();
976
            // add offset to current location
977
            target_loc.offset(ne_x, ne_y);
978
        }
979
            break;
980

981
        case MAV_FRAME_LOCAL_OFFSET_NED:
982
            // add offset to current location
983
            target_loc.offset(packet.x, packet.y);
984
            break;
985

986
        case MAV_FRAME_LOCAL_NED:
987
        default:
988
            // MAV_FRAME_LOCAL_NED is interpreted as an offset from EKF origin
989
            target_loc = ekf_origin;
990
            target_loc.offset(packet.x, packet.y);
991
            break;
992
        }
993
    }
994

995
    float target_speed = 0.0f;
996
    float target_yaw_cd = 0.0f;
997

998
    // consume velocity and convert to target speed and heading
999
    if (!vel_ignore) {
1000
        const float speed_max = rover.control_mode->get_speed_default();
1001
        // convert vector length into a speed
1002
        target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
1003
        // convert vector direction to target yaw
1004
        target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
1005

1006
        // rotate target yaw if provided in body-frame
1007
        if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
1008
            target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
1009
        }
1010
    }
1011

1012
    // consume yaw heading
1013
    if (!yaw_ignore) {
1014
        target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
1015
        // rotate target yaw if provided in body-frame
1016
        if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
1017
            target_yaw_cd = wrap_180_cd(target_yaw_cd + rover.ahrs.yaw_sensor);
1018
        }
1019
    }
1020
    // consume yaw rate
1021
    float target_turn_rate_cds = 0.0f;
1022
    if (!yaw_rate_ignore) {
1023
        target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
1024
    }
1025

1026
    // handling case when both velocity and either yaw or yaw-rate are provided
1027
    // by default, we consider that the rover will drive forward
1028
    float speed_dir = 1.0f;
1029
    if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
1030
        // Note: we are using the x-axis velocity to determine direction even though
1031
        // the frame may have been provided in MAV_FRAME_LOCAL_OFFSET_NED or MAV_FRAME_LOCAL_NED
1032
        if (is_negative(packet.vx)) {
1033
            speed_dir = -1.0f;
1034
        }
1035
    }
1036

1037
    // set guided mode targets
1038
    if (!pos_ignore) {
1039
        // consume position target
1040
        if (!rover.mode_guided.set_desired_location(target_loc)) {
1041
            // GCS will need to monitor desired location to
1042
            // see if they are having an effect.
1043
        }
1044
    } else if (!vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
1045
        // consume velocity
1046
        rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
1047
    } else if (!vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
1048
        // consume velocity and turn rate
1049
        rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
1050
    } else if (!vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
1051
        // consume velocity and heading
1052
        rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
1053
    } else if (vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
1054
        // consume just target heading (probably only skid steering vehicles can do this)
1055
        rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
1056
    } else if (vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
1057
        // consume just turn rate (probably only skid steering vehicles can do this)
1058
        rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
1059
    }
1060
}
1061

1062
void GCS_MAVLINK_Rover::handle_set_position_target_global_int(const mavlink_message_t &msg)
1063
{
1064
    // decode packet
1065
    mavlink_set_position_target_global_int_t packet;
1066
    mavlink_msg_set_position_target_global_int_decode(&msg, &packet);
1067

1068
    // exit if vehicle is not in Guided mode
1069
    if (!rover.control_mode->in_guided_mode()) {
1070
        return;
1071
    }
1072
    // check for supported coordinate frames
1073
    switch (packet.coordinate_frame) {
1074
    case MAV_FRAME_GLOBAL:
1075
    case MAV_FRAME_GLOBAL_INT:
1076
    case MAV_FRAME_GLOBAL_RELATIVE_ALT:
1077
    case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
1078
    case MAV_FRAME_GLOBAL_TERRAIN_ALT:
1079
    case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
1080
        break;
1081

1082
    default:
1083
        return;
1084
    }
1085
    
1086
    bool pos_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE;
1087
    bool vel_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE;
1088
    bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
1089
    bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
1090
    bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
1091

1092
    // prepare target position
1093
    Location target_loc = rover.current_loc;
1094
    if (!pos_ignore) {
1095
        // sanity check location
1096
        if (!check_latlng(packet.lat_int, packet.lon_int)) {
1097
            // result = MAV_RESULT_FAILED;
1098
            return;
1099
        }
1100
        target_loc.lat = packet.lat_int;
1101
        target_loc.lng = packet.lon_int;
1102
    }
1103

1104
    float target_speed = 0.0f;
1105
    float target_yaw_cd = 0.0f;
1106

1107
    // consume velocity and convert to target speed and heading
1108
    if (!vel_ignore) {
1109
        const float speed_max = rover.control_mode->get_speed_default();
1110
        // convert vector length into a speed
1111
        target_speed = constrain_float(safe_sqrt(sq(packet.vx) + sq(packet.vy)), -speed_max, speed_max);
1112
        // convert vector direction to target yaw
1113
        target_yaw_cd = degrees(atan2f(packet.vy, packet.vx)) * 100.0f;
1114
    }
1115

1116
    // consume yaw heading
1117
    if (!yaw_ignore) {
1118
        target_yaw_cd = ToDeg(packet.yaw) * 100.0f;
1119
    }
1120

1121
    // consume yaw rate
1122
    float target_turn_rate_cds = 0.0f;
1123
    if (!yaw_rate_ignore) {
1124
        target_turn_rate_cds = ToDeg(packet.yaw_rate) * 100.0f;
1125
    }
1126

1127
    // handling case when both velocity and either yaw or yaw-rate are provided
1128
    // by default, we consider that the rover will drive forward
1129
    float speed_dir = 1.0f;
1130
    if (!vel_ignore && (!yaw_ignore || !yaw_rate_ignore)) {
1131
        // Note: we are using the x-axis velocity to determine direction even though
1132
        // the frame is provided in MAV_FRAME_GLOBAL_xxx
1133
        if (is_negative(packet.vx)) {
1134
            speed_dir = -1.0f;
1135
        }
1136
    }
1137

1138
    // set guided mode targets
1139
    if (!pos_ignore) {
1140
        // consume position target
1141
        if (!rover.mode_guided.set_desired_location(target_loc)) {
1142
            // GCS will just need to look at desired location
1143
            // outputs to see if it having an effect.
1144
        }
1145
    } else if (!vel_ignore && acc_ignore && yaw_ignore && yaw_rate_ignore) {
1146
        // consume velocity
1147
        rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
1148
    } else if (!vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
1149
        // consume velocity and turn rate
1150
        rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, speed_dir * target_speed);
1151
    } else if (!vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
1152
        // consume velocity
1153
        rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, speed_dir * target_speed);
1154
    } else if (vel_ignore && acc_ignore && !yaw_ignore && yaw_rate_ignore) {
1155
        // consume just target heading (probably only skid steering vehicles can do this)
1156
        rover.mode_guided.set_desired_heading_and_speed(target_yaw_cd, 0.0f);
1157
    } else if (vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
1158
        // consume just turn rate(probably only skid steering vehicles can do this)
1159
        rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
1160
    }
1161
}
1162

1163
/*
1164
  handle a LANDING_TARGET command. The timestamp has been jitter corrected
1165
*/
1166
void GCS_MAVLINK_Rover::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
1167
{
1168
#if AC_PRECLAND_ENABLED
1169
    rover.precland.handle_msg(packet, timestamp_ms);
1170
#endif
1171
}
1172

1173
uint64_t GCS_MAVLINK_Rover::capabilities() const
1174
{
1175
    return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
1176
            MAV_PROTOCOL_CAPABILITY_MISSION_INT |
1177
            MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
1178
            MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
1179
            MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
1180
            MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
1181
            GCS_MAVLINK::capabilities());
1182
}
1183

1184
#if HAL_HIGH_LATENCY2_ENABLED
1185
uint8_t GCS_MAVLINK_Rover::high_latency_tgt_heading() const
1186
{
1187
    const Mode *control_mode = rover.control_mode;
1188
    if (rover.control_mode->is_autopilot_mode()) {
1189
        // need to convert -180->180 to 0->360/2
1190
        return wrap_360(control_mode->wp_bearing()) / 2;
1191
    }
1192
    return 0;
1193
}
1194
    
1195
uint16_t GCS_MAVLINK_Rover::high_latency_tgt_dist() const
1196
{
1197
    const Mode *control_mode = rover.control_mode;
1198
    if (rover.control_mode->is_autopilot_mode()) {
1199
        // return units are dm
1200
        return MIN((control_mode->get_distance_to_destination()) / 10, UINT16_MAX);
1201
    }
1202
    return 0;
1203
}
1204

1205
uint8_t GCS_MAVLINK_Rover::high_latency_tgt_airspeed() const
1206
{
1207
    const Mode *control_mode = rover.control_mode;
1208
    if (rover.control_mode->is_autopilot_mode()) {
1209
        // return units are m/s*5
1210
        return MIN((vfr_hud_airspeed() - control_mode->speed_error()) * 5, UINT8_MAX);
1211
    }
1212
    return 0;
1213
}
1214

1215
uint8_t GCS_MAVLINK_Rover::high_latency_wind_speed() const
1216
{
1217
    if (rover.g2.windvane.enabled()) {
1218
        // return units are m/s*5
1219
        return MIN(rover.g2.windvane.get_true_wind_speed() * 5, UINT8_MAX);
1220
    }
1221
    return 0;
1222
}
1223

1224
uint8_t GCS_MAVLINK_Rover::high_latency_wind_direction() const
1225
{
1226
    if (rover.g2.windvane.enabled()) {
1227
        // return units are deg/2
1228
        return wrap_360(degrees(rover.g2.windvane.get_true_wind_direction_rad())) / 2;
1229
    }
1230
    return 0;
1231
}
1232
#endif // HAL_HIGH_LATENCY2_ENABLED
1233

1234
// Send the mode with the given index (not mode number!) return the total number of modes
1235
// Index starts at 1
1236
uint8_t GCS_MAVLINK_Rover::send_available_mode(uint8_t index) const
1237
{
1238
    const Mode* modes[] {
1239
        &rover.mode_manual,
1240
        &rover.mode_acro,
1241
        &rover.mode_steering,
1242
        &rover.mode_hold,
1243
        &rover.mode_loiter,
1244
#if MODE_FOLLOW_ENABLED
1245
        &rover.mode_follow,
1246
#endif
1247
        &rover.mode_simple,
1248
        &rover.g2.mode_circle,
1249
        &rover.mode_auto,
1250
        &rover.mode_rtl,
1251
        &rover.mode_smartrtl,
1252
        &rover.mode_guided,
1253
        &rover.mode_initializing,
1254
#if MODE_DOCK_ENABLED
1255
        (Mode *)rover.g2.mode_dock_ptr,
1256
#endif
1257
    };
1258

1259
    const uint8_t mode_count = ARRAY_SIZE(modes);
1260

1261
    // Convert to zero indexed
1262
    const uint8_t index_zero = index - 1;
1263
    if (index_zero >= mode_count) {
1264
        // Mode does not exist!?
1265
        return mode_count;
1266
    }
1267

1268
    // Ask the mode for its name and number
1269
    const char* name = modes[index_zero]->name4();
1270
    const uint8_t mode_number = (uint8_t)modes[index_zero]->mode_number();
1271

1272
    mavlink_msg_available_modes_send(
1273
        chan,
1274
        mode_count,
1275
        index,
1276
        MAV_STANDARD_MODE::MAV_STANDARD_MODE_NON_STANDARD,
1277
        mode_number,
1278
        0, // MAV_MODE_PROPERTY bitmask
1279
        name
1280
    );
1281

1282
    return mode_count;
1283
}
1284

1285
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

Product

Resources

Company

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more, all in one place. Commercial Alternative to JupyterHub.

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
