ArduPlane/GCS_Mavlink.cpp

   1 #include "GCS_Mavlink.h"
   2
   3 #include "Plane.h"
   4 #include <AP_RPM/AP_RPM_config.h>
   5 #include <AP_Airspeed/AP_Airspeed_config.h>
   6 #include <AP_EFI/AP_EFI_config.h>
   7
   8 MAV_TYPE GCS_Plane::frame_type() const
   9 {
  10 #if HAL_QUADPLANE_ENABLED
  11     return plane.quadplane.get_mav_type();
  12 #else
  13     return MAV_TYPE_FIXED_WING;
  14 #endif
  15 }
  16
  17 MAV_MODE GCS_MAVLINK_Plane::base_mode() const
  18 {
  19     uint8_t _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
  20
  21     // work out the base_mode. This value is not very useful
  22     // for APM, but we calculate it as best we can so a generic
  23     // MAVLink enabled ground station can work out something about
  24     // what the MAV is up to. The actual bit values are highly
  25     // ambiguous for most of the APM flight modes. In practice, you
  26     // only get useful information from the custom_mode, which maps to
  27     // the APM flight mode and has a well defined meaning in the
  28     // ArduPlane documentation
  29     switch (plane.control_mode->mode_number()) {
  30     case Mode::Number::MANUAL:
  31     case Mode::Number::TRAINING:
  32     case Mode::Number::ACRO:
  33 #if HAL_QUADPLANE_ENABLED
  34     case Mode::Number::QACRO:
  35         _base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
  36         break;
  37 #endif
  38     case Mode::Number::STABILIZE:
  39     case Mode::Number::FLY_BY_WIRE_A:
  40     case Mode::Number::AUTOTUNE:
  41     case Mode::Number::FLY_BY_WIRE_B:
  42 #if HAL_QUADPLANE_ENABLED
  43     case Mode::Number::QSTABILIZE:
  44     case Mode::Number::QHOVER:
  45     case Mode::Number::QLOITER:
  46     case Mode::Number::QLAND:
  47 #if QAUTOTUNE_ENABLED
  48     case Mode::Number::QAUTOTUNE:
  49 #endif
  50 #endif  // HAL_QUADPLANE_ENABLED
  51     case Mode::Number::CRUISE:
  52         _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
  53         break;
  54     case Mode::Number::AUTO:
  55     case Mode::Number::RTL:
  56     case Mode::Number::LOITER:
  57     case Mode::Number::THERMAL:
  58     case Mode::Number::AVOID_ADSB:
  59     case Mode::Number::GUIDED:
  60     case Mode::Number::CIRCLE:
  61     case Mode::Number::TAKEOFF:
  62 #if HAL_QUADPLANE_ENABLED
  63     case Mode::Number::QRTL:
  64     case Mode::Number::LOITER_ALT_QLAND:
  65 #endif
  66         _base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
  67                      MAV_MODE_FLAG_STABILIZE_ENABLED;
  68         // note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
  69         // APM does in any mode, as that is defined as "system finds its own goal
  70         // positions", which APM does not currently do
  71         break;
  72     case Mode::Number::INITIALISING:
  73         break;
  74     }
  75
  76     if (!plane.training_manual_pitch || !plane.training_manual_roll) {
  77         _base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
  78     }
  79
  80     if (plane.control_mode != &plane.mode_manual && plane.control_mode != &plane.mode_initializing) {
  81         // stabiliser of some form is enabled
  82         _base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
  83     }
  84
  85     if (plane.g.stick_mixing != StickMixing::NONE && plane.control_mode != &plane.mode_initializing) {
  86         if ((plane.g.stick_mixing != StickMixing::VTOL_YAW) || (plane.control_mode == &plane.mode_auto)) {
  87             // all modes except INITIALISING have some form of manual
  88             // override if stick mixing is enabled
  89             _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
  90         }
  91     }
  92
  93     // we are armed if we are not initialising
  94     if (plane.control_mode != &plane.mode_initializing && plane.arming.is_armed()) {
  95         _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
  96     }
  97
  98     // indicate we have set a custom mode
  99     _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 100
 101     return (MAV_MODE)_base_mode;
 102 }
 103
 104 uint32_t GCS_Plane::custom_mode() const
 105 {
 106     return plane.control_mode->mode_number();
 107 }
 108
 109 MAV_STATE GCS_MAVLINK_Plane::vehicle_system_status() const
 110 {
 111     if (plane.control_mode == &plane.mode_initializing) {
 112         return MAV_STATE_CALIBRATING;
 113     }
 114     if (plane.any_failsafe_triggered()) {
 115         return MAV_STATE_CRITICAL;
 116     }
 117     if (plane.crash_state.is_crashed) {
 118         return MAV_STATE_EMERGENCY;
 119     }
 120     if (plane.is_flying()) {
 121         return MAV_STATE_ACTIVE;
 122     }
 123
 124     return MAV_STATE_STANDBY;
 125 }
 126
 127
 128 void GCS_MAVLINK_Plane::send_attitude() const
 129 {
 130     const AP_AHRS &ahrs = AP::ahrs();
 131
 132     float r = ahrs.get_roll();
 133     float p = ahrs.get_pitch();
 134     float y = ahrs.get_yaw();
 135
 136     if (!(plane.flight_option_enabled(FlightOptions::GCS_REMOVE_TRIM_PITCH))) {
 137         p -= radians(plane.g.pitch_trim);
 138     }
 139
 140 #if HAL_QUADPLANE_ENABLED
 141     if (plane.quadplane.show_vtol_view()) {
 142         r = plane.quadplane.ahrs_view->roll;
 143         p = plane.quadplane.ahrs_view->pitch;
 144         y = plane.quadplane.ahrs_view->yaw;
 145     }
 146 #endif
 147
 148     const Vector3f &omega = ahrs.get_gyro();
 149     mavlink_msg_attitude_send(
 150         chan,
 151         millis(),
 152         r,
 153         p,
 154         y,
 155         omega.x,
 156         omega.y,
 157         omega.z);
 158 }
 159
 160 void GCS_MAVLINK_Plane::send_attitude_target()
 161 {
 162 #if HAL_QUADPLANE_ENABLED
 163     // Check if the attitude target is valid for reporting
 164     const uint32_t now = AP_HAL::millis();
 165     if (now  - plane.quadplane.last_att_control_ms > 100) {
 166         return;
 167     }
 168
 169     const Quaternion quat  = plane.quadplane.attitude_control->get_attitude_target_quat();
 170     const Vector3f& ang_vel = plane.quadplane.attitude_control->get_attitude_target_ang_vel();
 171     const float throttle = plane.quadplane.attitude_control->get_throttle_in();
 172
 173     const float quat_out[4] {quat.q1, quat.q2, quat.q3, quat.q4};
 174
 175     const uint16_t typemask = 0;
 176
 177     mavlink_msg_attitude_target_send(
 178         chan,
 179         now,                    // time since boot (ms)
 180         typemask,               // Bitmask that tells the system what control dimensions should be ignored by the vehicle
 181         quat_out,               // Target attitude quaternion [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], unit-length
 182         ang_vel.x,              // bodyframe target roll rate (rad/s)
 183         ang_vel.y,              // bodyframe target pitch rate (rad/s)
 184         ang_vel.z,              // bodyframe yaw rate (rad/s)
 185         throttle);              // Collective thrust, normalized to 0 .. 1
 186
 187 #endif // HAL_QUADPLANE_ENABLED
 188 }
 189
 190 void GCS_MAVLINK_Plane::send_aoa_ssa()
 191 {
 192     AP_AHRS &ahrs = AP::ahrs();
 193
 194     mavlink_msg_aoa_ssa_send(
 195         chan,
 196         micros(),
 197         ahrs.getAOA(),
 198         ahrs.getSSA());
 199 }
 200
 201 void GCS_MAVLINK_Plane::send_nav_controller_output() const
 202 {
 203     if (plane.control_mode == &plane.mode_manual) {
 204         return;
 205     }
 206 #if HAL_QUADPLANE_ENABLED
 207     const QuadPlane &quadplane = plane.quadplane;
 208     if (quadplane.show_vtol_view() && quadplane.using_wp_nav()) {
 209         const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
 210
 211         const Vector2f& curr_pos = quadplane.inertial_nav.get_position_xy_cm();
 212         const Vector2f& target_pos = quadplane.pos_control->get_pos_target_cm().xy().tofloat();
 213         const Vector2f error = (target_pos - curr_pos) * 0.01;
 214
 215         mavlink_msg_nav_controller_output_send(
 216             chan,
 217             targets.x * 0.01,
 218             targets.y * 0.01,
 219             targets.z * 0.01,
 220             degrees(error.angle()),
 221             MIN(error.length(), UINT16_MAX),
 222             (plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_pos_error_z_cm() * 0.01 : 0,
 223             plane.airspeed_error * 100,  // incorrect units; see PR#7933
 224             quadplane.wp_nav->crosstrack_error());
 225         return;
 226     }
 227 #endif
 228     {
 229         const AP_Navigation *nav_controller = plane.nav_controller;
 230         mavlink_msg_nav_controller_output_send(
 231             chan,
 232             plane.nav_roll_cd * 0.01,
 233             plane.nav_pitch_cd * 0.01,
 234             nav_controller->nav_bearing_cd() * 0.01,
 235             nav_controller->target_bearing_cd() * 0.01,
 236             MIN(plane.auto_state.wp_distance, UINT16_MAX),
 237             plane.calc_altitude_error_cm() * 0.01,
 238             plane.airspeed_error * 100,  // incorrect units; see PR#7933
 239             nav_controller->crosstrack_error());
 240     }
 241 }
 242
 243 void GCS_MAVLINK_Plane::send_position_target_global_int()
 244 {
 245     if (plane.control_mode == &plane.mode_manual) {
 246         return;
 247     }
 248     Location &next_WP_loc = plane.next_WP_loc;
 249     static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000;
 250     static constexpr uint16_t TYPE_MASK = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
 251                                           POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE |
 252                                           POSITION_TARGET_TYPEMASK_YAW_IGNORE | POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE | POSITION_TARGET_TYPEMASK_LAST_BYTE;
 253     int32_t alt = 0;
 254     if (!next_WP_loc.is_zero()) {
 255         UNUSED_RESULT(next_WP_loc.get_alt_cm(Location::AltFrame::ABSOLUTE, alt));
 256     }
 257
 258     mavlink_msg_position_target_global_int_send(
 259         chan,
 260         AP_HAL::millis(), // time_boot_ms
 261         MAV_FRAME_GLOBAL, // targets are always global altitude
 262         TYPE_MASK, // ignore everything except the x/y/z components
 263         next_WP_loc.lat, // latitude as 1e7
 264         next_WP_loc.lng, // longitude as 1e7
 265         alt * 0.01, // altitude is sent as a float
 266         0.0f, // vx
 267         0.0f, // vy
 268         0.0f, // vz
 269         0.0f, // afx
 270         0.0f, // afy
 271         0.0f, // afz
 272         0.0f, // yaw
 273         0.0f); // yaw_rate
 274 }
 275
 276
 277 float GCS_MAVLINK_Plane::vfr_hud_airspeed() const
 278 {
 279     // airspeed sensors are best.  While the AHRS airspeed_estimate
 280     // will use an airspeed sensor, that value is constrained by the
 281     // ground speed.  When reporting we should send the true airspeed
 282     // value if possible:
 283 #if AP_AIRSPEED_ENABLED
 284     if (plane.airspeed.enabled() && plane.airspeed.healthy()) {
 285         return plane.airspeed.get_airspeed();
 286     }
 287 #endif
 288
 289     // airspeed estimates are OK:
 290     float aspeed;
 291     if (AP::ahrs().airspeed_estimate(aspeed)) {
 292         return aspeed;
 293     }
 294
 295     // lying is worst:
 296     return 0;
 297 }
 298
 299 int16_t GCS_MAVLINK_Plane::vfr_hud_throttle() const
 300 {
 301     return plane.throttle_percentage();
 302 }
 303
 304 float GCS_MAVLINK_Plane::vfr_hud_climbrate() const
 305 {
 306 #if HAL_SOARING_ENABLED
 307     if (plane.g2.soaring_controller.is_active()) {
 308         return plane.g2.soaring_controller.get_vario_reading();
 309     }
 310 #endif
 311     return GCS_MAVLINK::vfr_hud_climbrate();
 312 }
 313
 314 void GCS_MAVLINK_Plane::send_wind() const
 315 {
 316     const Vector3f wind = AP::ahrs().wind_estimate();
 317     mavlink_msg_wind_send(
 318         chan,
 319         degrees(atan2f(-wind.y, -wind.x)), // use negative, to give
 320                                           // direction wind is coming from
 321         wind.length(),
 322         wind.z);
 323 }
 324
 325 // sends a single pid info over the provided channel
 326 void GCS_MAVLINK_Plane::send_pid_info(const AP_PIDInfo *pid_info,
 327                           const uint8_t axis, const float achieved)
 328 {
 329     if (pid_info == nullptr) {
 330         return;
 331     }
 332     if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
 333         return;
 334     }
 335      mavlink_msg_pid_tuning_send(chan, axis,
 336                                  pid_info->target,
 337                                  achieved,
 338                                  pid_info->FF,
 339                                  pid_info->P,
 340                                  pid_info->I,
 341                                  pid_info->D,
 342                                  pid_info->slew_rate,
 343                                  pid_info->Dmod);
 344 }
 345
 346 /*
 347   send PID tuning message
 348  */
 349 void GCS_MAVLINK_Plane::send_pid_tuning()
 350 {
 351     if (plane.control_mode == &plane.mode_manual) {
 352         // no PIDs should be used in manual
 353         return;
 354     }
 355
 356     const Parameters &g = plane.g;
 357
 358     const AP_PIDInfo *pid_info;
 359     if (g.gcs_pid_mask & TUNING_BITS_ROLL) {
 360         pid_info = &plane.rollController.get_pid_info();
 361 #if HAL_QUADPLANE_ENABLED
 362         if (plane.quadplane.in_vtol_mode()) {
 363             pid_info = &plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
 364         }
 365 #endif
 366         send_pid_info(pid_info, PID_TUNING_ROLL, pid_info->actual);
 367     }
 368     if (g.gcs_pid_mask & TUNING_BITS_PITCH) {
 369         pid_info = &plane.pitchController.get_pid_info();
 370 #if HAL_QUADPLANE_ENABLED
 371         if (plane.quadplane.in_vtol_mode()) {
 372             pid_info = &plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
 373         }
 374 #endif
 375         send_pid_info(pid_info, PID_TUNING_PITCH, pid_info->actual);
 376     }
 377     if (g.gcs_pid_mask & TUNING_BITS_YAW) {
 378         pid_info = &plane.yawController.get_pid_info();
 379 #if HAL_QUADPLANE_ENABLED
 380         if (plane.quadplane.in_vtol_mode()) {
 381             pid_info = &plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info();
 382         }
 383 #endif
 384         send_pid_info(pid_info, PID_TUNING_YAW, pid_info->actual);
 385     }
 386     if (g.gcs_pid_mask & TUNING_BITS_STEER) {
 387         pid_info = &plane.steerController.get_pid_info();
 388         send_pid_info(pid_info, PID_TUNING_STEER, pid_info->actual);
 389     }
 390     if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (plane.flight_stage == AP_FixedWing::FlightStage::LAND)) {
 391         AP_AHRS &ahrs = AP::ahrs();
 392         const Vector3f &gyro = ahrs.get_gyro();
 393         send_pid_info(plane.landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z));
 394     }
 395 #if HAL_QUADPLANE_ENABLED
 396     if (g.gcs_pid_mask & TUNING_BITS_ACCZ && plane.quadplane.in_vtol_mode()) {
 397         pid_info = &plane.quadplane.pos_control->get_accel_z_pid().get_pid_info();
 398         send_pid_info(pid_info, PID_TUNING_ACCZ, pid_info->actual);
 399     }
 400 #endif
 401  }
 402
 403 uint8_t GCS_MAVLINK_Plane::sysid_my_gcs() const
 404 {
 405     return plane.g.sysid_my_gcs;
 406 }
 407 bool GCS_MAVLINK_Plane::sysid_enforce() const
 408 {
 409     return plane.g2.sysid_enforce;
 410 }
 411
 412 uint32_t GCS_MAVLINK_Plane::telem_delay() const
 413 {
 414     return (uint32_t)(plane.g.telem_delay);
 415 }
 416
 417 // try to send a message, return false if it won't fit in the serial tx buffer
 418 bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
 419 {
 420     switch (id) {
 421
 422     case MSG_SERVO_OUT:
 423         // unused
 424         break;
 425
 426     case MSG_TERRAIN:
 427 #if AP_TERRAIN_AVAILABLE
 428         CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST);
 429         plane.terrain.send_request(chan);
 430 #endif
 431         break;
 432
 433     case MSG_WIND:
 434         CHECK_PAYLOAD_SIZE(WIND);
 435         send_wind();
 436         break;
 437
 438     case MSG_ADSB_VEHICLE:
 439 #if HAL_ADSB_ENABLED
 440         CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
 441         plane.adsb.send_adsb_vehicle(chan);
 442 #endif
 443         break;
 444
 445     case MSG_AOA_SSA:
 446         CHECK_PAYLOAD_SIZE(AOA_SSA);
 447         send_aoa_ssa();
 448         break;
 449     case MSG_LANDING:
 450         plane.landing.send_landing_message(chan);
 451         break;
 452
 453     case MSG_HYGROMETER:
 454 #if AP_AIRSPEED_HYGROMETER_ENABLE
 455         CHECK_PAYLOAD_SIZE(HYGROMETER_SENSOR);
 456         send_hygrometer();
 457 #endif
 458         break;
 459
 460     default:
 461         return GCS_MAVLINK::try_send_message(id);
 462     }
 463     return true;
 464 }
 465
 466 #if AP_AIRSPEED_HYGROMETER_ENABLE
 467 void GCS_MAVLINK_Plane::send_hygrometer()
 468 {
 469     if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) {
 470         return;
 471     }
 472
 473     const auto *airspeed = AP::airspeed();
 474     if (airspeed == nullptr) {
 475         return;
 476     }
 477     const uint32_t now = AP_HAL::millis();
 478
 479     for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
 480         uint8_t idx = (i+last_hygrometer_send_idx+1) % AIRSPEED_MAX_SENSORS;
 481         float temperature, humidity;
 482         uint32_t last_sample_ms;
 483         if (!airspeed->get_hygrometer(idx, last_sample_ms, temperature, humidity)) {
 484             continue;
 485         }
 486         if (now - last_sample_ms > 2000) {
 487             // not updating, stop sending
 488             continue;
 489         }
 490         if (!HAVE_PAYLOAD_SPACE(chan, HYGROMETER_SENSOR)) {
 491             return;
 492         }
 493
 494         mavlink_msg_hygrometer_sensor_send(
 495             chan,
 496             idx,
 497             int16_t(temperature*100),
 498             uint16_t(humidity*100));
 499         last_hygrometer_send_idx = idx;
 500     }
 501 }
 502 #endif // AP_AIRSPEED_HYGROMETER_ENABLE
 503
 504
 505 /*
 506   default stream rates to 1Hz
 507  */
 508 const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
 509     // @Param: RAW_SENS
 510     // @DisplayName: Raw sensor stream rate
 511     // @Description: MAVLink Stream rate of RAW_IMU, SCALED_IMU2, SCALED_IMU3, SCALED_PRESSURE, SCALED_PRESSURE2, and SCALED_PRESSURE3
 512     // @Units: Hz
 513     // @Range: 0 50
 514     // @Increment: 1
 515     // @RebootRequired: True
 516     // @User: Advanced
 517     AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0],  1),
 518
 519     // @Param: EXT_STAT
 520     // @DisplayName: Extended status stream rate
 521     // @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
 522     // @Units: Hz
 523     // @Range: 0 50
 524     // @Increment: 1
 525     // @RebootRequired: True
 526     // @User: Advanced
 527     AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1],  1),
 528
 529     // @Param: RC_CHAN
 530     // @DisplayName: RC Channel stream rate
 531     // @Description: MAVLink Stream rate of SERVO_OUTPUT_RAW and RC_CHANNELS
 532     // @Units: Hz
 533     // @Range: 0 50
 534     // @Increment: 1
 535     // @RebootRequired: True
 536     // @User: Advanced
 537     AP_GROUPINFO("RC_CHAN",  2, GCS_MAVLINK_Parameters, streamRates[2],  1),
 538
 539     // @Param: RAW_CTRL
 540     // @DisplayName: Raw Control stream rate
 541     // @Description: MAVLink Raw Control stream rate of SERVO_OUT
 542     // @Units: Hz
 543     // @Range: 0 50
 544     // @Increment: 1
 545     // @RebootRequired: True
 546     // @User: Advanced
 547     AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3],  1),
 548
 549     // @Param: POSITION
 550     // @DisplayName: Position stream rate
 551     // @Description: MAVLink Stream rate of GLOBAL_POSITION_INT and LOCAL_POSITION_NED
 552     // @Units: Hz
 553     // @Range: 0 50
 554     // @Increment: 1
 555     // @RebootRequired: True
 556     // @User: Advanced
 557     AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4],  1),
 558
 559     // @Param: EXTRA1
 560     // @DisplayName: Extra data type 1 stream rate
 561     // @Description: MAVLink Stream rate of ATTITUDE, SIMSTATE (SIM only), AHRS2, RPM, AOA_SSA, LANDING,ESC_TELEMETRY,EFI_STATUS, and PID_TUNING
 562     // @Units: Hz
 563     // @Range: 0 50
 564     // @Increment: 1
 565     // @RebootRequired: True
 566     // @User: Advanced
 567     AP_GROUPINFO("EXTRA1",   5, GCS_MAVLINK_Parameters, streamRates[5],  1),
 568
 569     // @Param: EXTRA2
 570     // @DisplayName: Extra data type 2 stream rate
 571     // @Description: MAVLink Stream rate of VFR_HUD
 572     // @Range: 0 50
 573     // @Increment: 1
 574     // @RebootRequired: True
 575     // @User: Advanced
 576     AP_GROUPINFO("EXTRA2",   6, GCS_MAVLINK_Parameters, streamRates[6],  1),
 577
 578     // @Param: EXTRA3
 579     // @DisplayName: Extra data type 3 stream rate
 580     // @Description: MAVLink Stream rate of AHRS, SYSTEM_TIME, WIND, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, BATTERY2, GIMBAL_DEVICE_ATTITUDE_STATUS, OPTICAL_FLOW, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION, and BATTERY_STATUS
 581     // @Units: Hz
 582     // @Range: 0 50
 583     // @Increment: 1
 584     // @RebootRequired: True
 585     // @User: Advanced
 586     AP_GROUPINFO("EXTRA3",   7, GCS_MAVLINK_Parameters, streamRates[7],  1),
 587
 588     // @Param: PARAMS
 589     // @DisplayName: Parameter stream rate
 590     // @Description: MAVLink Stream rate of PARAM_VALUE
 591     // @Units: Hz
 592     // @Range: 0 50
 593     // @Increment: 1
 594     // @RebootRequired: True
 595     // @User: Advanced
 596     AP_GROUPINFO("PARAMS",   8, GCS_MAVLINK_Parameters, streamRates[8],  10),
 597
 598     // @Param: ADSB
 599     // @DisplayName: ADSB stream rate
 600     // @Description: MAVLink ADSB stream rate
 601     // @Units: Hz
 602     // @Range: 0 50
 603     // @Increment: 1
 604     // @RebootRequired: True
 605     // @User: Advanced
 606     AP_GROUPINFO("ADSB",   9, GCS_MAVLINK_Parameters, streamRates[9],  5),
 607     AP_GROUPEND
 608 };
 609
 610 static const ap_message STREAM_RAW_SENSORS_msgs[] = {
 611     MSG_RAW_IMU,
 612     MSG_SCALED_IMU2,
 613     MSG_SCALED_IMU3,
 614     MSG_SCALED_PRESSURE,
 615     MSG_SCALED_PRESSURE2,
 616     MSG_SCALED_PRESSURE3,
 617 };
 618 static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
 619     MSG_SYS_STATUS,
 620     MSG_POWER_STATUS,
 621 #if HAL_WITH_MCU_MONITORING
 622     MSG_MCU_STATUS,
 623 #endif
 624     MSG_MEMINFO,
 625     MSG_CURRENT_WAYPOINT,
 626     MSG_GPS_RAW,
 627     MSG_GPS_RTK,
 628 #if GPS_MAX_RECEIVERS > 1
 629     MSG_GPS2_RAW,
 630     MSG_GPS2_RTK,
 631 #endif
 632     MSG_NAV_CONTROLLER_OUTPUT,
 633 #if AP_FENCE_ENABLED
 634     MSG_FENCE_STATUS,
 635 #endif
 636     MSG_POSITION_TARGET_GLOBAL_INT,
 637 };
 638 static const ap_message STREAM_POSITION_msgs[] = {
 639     MSG_LOCATION,
 640     MSG_LOCAL_POSITION
 641 };
 642 static const ap_message STREAM_RAW_CONTROLLER_msgs[] = {
 643     MSG_SERVO_OUT,
 644 };
 645 static const ap_message STREAM_RC_CHANNELS_msgs[] = {
 646     MSG_SERVO_OUTPUT_RAW,
 647     MSG_RC_CHANNELS,
 648     MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
 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 #if AP_RPM_ENABLED
 657     MSG_RPM,
 658 #endif
 659     MSG_AOA_SSA,
 660     MSG_PID_TUNING,
 661     MSG_LANDING,
 662 #if HAL_WITH_ESC_TELEM
 663     MSG_ESC_TELEMETRY,
 664 #endif
 665 #if HAL_EFI_ENABLED
 666     MSG_EFI_STATUS,
 667 #endif
 668 #if AP_AIRSPEED_HYGROMETER_ENABLE
 669     MSG_HYGROMETER,
 670 #endif
 671 };
 672 static const ap_message STREAM_EXTRA2_msgs[] = {
 673     MSG_VFR_HUD
 674 };
 675 static const ap_message STREAM_EXTRA3_msgs[] = {
 676     MSG_AHRS,
 677     MSG_WIND,
 678 #if AP_RANGEFINDER_ENABLED
 679     MSG_RANGEFINDER,
 680 #endif
 681     MSG_DISTANCE_SENSOR,
 682     MSG_SYSTEM_TIME,
 683 #if AP_TERRAIN_AVAILABLE
 684     MSG_TERRAIN,
 685 #endif
 686 #if AP_BATTERY_ENABLED
 687     MSG_BATTERY_STATUS,
 688 #endif
 689 #if HAL_MOUNT_ENABLED
 690     MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
 691 #endif
 692 #if AP_OPTICALFLOW_ENABLED
 693     MSG_OPTICAL_FLOW,
 694 #endif
 695 #if COMPASS_CAL_ENABLED
 696     MSG_MAG_CAL_REPORT,
 697     MSG_MAG_CAL_PROGRESS,
 698 #endif
 699     MSG_EKF_STATUS_REPORT,
 700     MSG_VIBRATION,
 701 };
 702 static const ap_message STREAM_PARAMS_msgs[] = {
 703     MSG_NEXT_PARAM
 704 };
 705 static const ap_message STREAM_ADSB_msgs[] = {
 706     MSG_ADSB_VEHICLE,
 707 #if AP_AIS_ENABLED
 708     MSG_AIS_VESSEL,
 709 #endif
 710 };
 711
 712 const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
 713     MAV_STREAM_ENTRY(STREAM_RAW_SENSORS),
 714     MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS),
 715     MAV_STREAM_ENTRY(STREAM_POSITION),
 716     MAV_STREAM_ENTRY(STREAM_RAW_CONTROLLER),
 717     MAV_STREAM_ENTRY(STREAM_RC_CHANNELS),
 718     MAV_STREAM_ENTRY(STREAM_EXTRA1),
 719     MAV_STREAM_ENTRY(STREAM_EXTRA2),
 720     MAV_STREAM_ENTRY(STREAM_EXTRA3),
 721     MAV_STREAM_ENTRY(STREAM_PARAMS),
 722     MAV_STREAM_ENTRY(STREAM_ADSB),
 723     MAV_STREAM_TERMINATOR // must have this at end of stream_entries
 724 };
 725
 726 /*
 727   handle a request to switch to guided mode. This happens via a
 728   callback from handle_mission_item()
 729  */
 730 bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd)
 731 {
 732     return plane.control_mode->handle_guided_request(cmd.content.location);
 733 }
 734
 735 /*
 736   handle a request to change current WP altitude. This happens via a
 737   callback from handle_mission_item()
 738  */
 739 void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
 740 {
 741     plane.next_WP_loc.alt = cmd.content.location.alt;
 742     if (cmd.content.location.relative_alt) {
 743         plane.next_WP_loc.alt += plane.home.alt;
 744     }
 745     plane.next_WP_loc.relative_alt = false;
 746     plane.next_WP_loc.terrain_alt = cmd.content.location.terrain_alt;
 747     plane.reset_offset_altitude();
 748 }
 749
 750
 751 /*
 752   handle a LANDING_TARGET command. The timestamp has been jitter corrected
 753 */
 754 void GCS_MAVLINK_Plane::handle_landing_target(const mavlink_landing_target_t &packet, uint32_t timestamp_ms)
 755 {
 756 #if AC_PRECLAND_ENABLED
 757     plane.g2.precland.handle_msg(packet, timestamp_ms);
 758 #endif
 759 }
 760
 761 MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
 762 {
 763     plane.in_calibration = true;
 764     MAV_RESULT ret = GCS_MAVLINK::handle_command_preflight_calibration(packet, msg);
 765     plane.in_calibration = false;
 766
 767     return ret;
 768 }
 769
 770 void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
 771                                        const mavlink_message_t &msg)
 772 {
 773 #if HAL_ADSB_ENABLED
 774     plane.avoidance_adsb.handle_msg(msg);
 775 #endif
 776 #if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED
 777     // pass message to follow library
 778     plane.g2.follow.handle_msg(msg);
 779 #endif
 780     GCS_MAVLINK::packetReceived(status, msg);
 781 }
 782
 783
 784 bool Plane::set_home_to_current_location(bool _lock)
 785 {
 786     if (!set_home_persistently(AP::gps().location())) {
 787         return false;
 788     }
 789     if (_lock) {
 790         AP::ahrs().lock_home();
 791     }
 792     if ((control_mode == &mode_rtl)
 793 #if HAL_QUADPLANE_ENABLED
 794             || (control_mode == &mode_qrtl)
 795 #endif
 796                                                         ) {
 797         // if in RTL head to the updated home location
 798         control_mode->enter();
 799     }
 800     return true;
 801 }
 802 bool Plane::set_home(const Location& loc, bool _lock)
 803 {
 804     if (!AP::ahrs().set_home(loc)) {
 805         return false;
 806     }
 807     if (_lock) {
 808         AP::ahrs().lock_home();
 809     }
 810     if ((control_mode == &mode_rtl)
 811 #if HAL_QUADPLANE_ENABLED
 812             || (control_mode == &mode_qrtl)
 813 #endif
 814                                                         ) {
 815         // if in RTL head to the updated home location
 816         control_mode->enter();
 817     }
 818     return true;
 819 }
 820
 821 MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
 822 {
 823     // sanity check location
 824     if (!check_latlng(packet.x, packet.y)) {
 825         return MAV_RESULT_DENIED;
 826     }
 827
 828     Location requested_position;
 829     if (!location_from_command_t(packet, requested_position)) {
 830         return MAV_RESULT_DENIED;
 831     }
 832
 833     if (isnan(packet.param4) || is_zero(packet.param4)) {
 834         requested_position.loiter_ccw = 0;
 835     } else {
 836         requested_position.loiter_ccw = 1;
 837     }
 838
 839     if (requested_position.sanitize(plane.current_loc)) {
 840         // if the location wasn't already sane don't load it
 841         return MAV_RESULT_DENIED;
 842     }
 843
 844     // location is valid load and set
 845     if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
 846         (plane.control_mode == &plane.mode_guided)) {
 847         plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
 848
 849         // add home alt if needed
 850         if (requested_position.relative_alt) {
 851             requested_position.alt += plane.home.alt;
 852             requested_position.relative_alt = 0;
 853         }
 854
 855         plane.set_guided_WP(requested_position);
 856
 857         // Loiter radius for planes. Positive radius in meters, direction is controlled by Yaw (param4) value, parsed above
 858         if (!isnan(packet.param3) && packet.param3 > 0) {
 859             plane.mode_guided.set_radius_and_direction(packet.param3, requested_position.loiter_ccw);
 860         }
 861
 862         return MAV_RESULT_ACCEPTED;
 863     }
 864     return MAV_RESULT_FAILED;
 865 }
 866
 867 // these are GUIDED mode commands that are RATE or slew enabled, so you can have more powerful control than default controls.
 868 MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet)
 869 {
 870   switch(packet.command) {
 871
 872 #if OFFBOARD_GUIDED == ENABLED
 873     case MAV_CMD_GUIDED_CHANGE_SPEED: {
 874         // command is only valid in guided mode
 875         if (plane.control_mode != &plane.mode_guided) {
 876             return MAV_RESULT_FAILED;
 877         }
 878
 879          // only airspeed commands are supported right now...
 880         if (int(packet.param1) != SPEED_TYPE_AIRSPEED) {  // since SPEED_TYPE is int in range 0-1 and packet.param1 is a *float* this works.
 881             return MAV_RESULT_DENIED;
 882         }
 883
 884          // reject airspeeds that are outside of the tuning envelope
 885         if (packet.param2 > plane.aparm.airspeed_max || packet.param2 < plane.aparm.airspeed_min) {
 886             return MAV_RESULT_DENIED;
 887         }
 888
 889          // no need to process any new packet/s with the
 890          //  same airspeed any further, if we are already doing it.
 891         float new_target_airspeed_cm = packet.param2 * 100;
 892         if ( is_equal(new_target_airspeed_cm,plane.guided_state.target_airspeed_cm)) {
 893             return MAV_RESULT_ACCEPTED;
 894         }
 895         plane.guided_state.target_airspeed_cm = new_target_airspeed_cm;
 896         plane.guided_state.target_airspeed_time_ms = AP_HAL::millis();
 897
 898          if (is_zero(packet.param3)) {
 899             // the user wanted /maximum acceleration, pick a large value as close enough
 900             plane.guided_state.target_airspeed_accel = 1000.0f;
 901         } else {
 902             plane.guided_state.target_airspeed_accel = fabsf(packet.param3);
 903         }
 904
 905          // assign an acceleration direction
 906         if (plane.guided_state.target_airspeed_cm < plane.target_airspeed_cm) {
 907             plane.guided_state.target_airspeed_accel *= -1.0f;
 908         }
 909         return MAV_RESULT_ACCEPTED;
 910     }
 911
 912      case MAV_CMD_GUIDED_CHANGE_ALTITUDE: {
 913         // command is only valid in guided
 914         if (plane.control_mode != &plane.mode_guided) {
 915             return MAV_RESULT_FAILED;
 916         }
 917
 918         // disallow default value of -1 and dangerous value of zero
 919         if (is_equal(packet.z, -1.0f) || is_equal(packet.z, 0.0f)){
 920             return MAV_RESULT_DENIED;
 921         }
 922
 923          // the requested alt data might be relative or absolute
 924         float new_target_alt = packet.z * 100;
 925         float new_target_alt_rel = packet.z * 100 + plane.home.alt;
 926
 927          // only global/relative/terrain frames are supported
 928         switch(packet.frame) {
 929             case MAV_FRAME_GLOBAL_RELATIVE_ALT: {
 930                 if   (is_equal(plane.guided_state.target_alt,new_target_alt_rel) ) { // compare two floats as near-enough
 931                     // no need to process any new packet/s with the same ALT any further, if we are already doing it.
 932                     return MAV_RESULT_ACCEPTED;
 933                 }
 934                 plane.guided_state.target_alt = new_target_alt_rel;
 935                 break;
 936             }
 937             case MAV_FRAME_GLOBAL: {
 938                 if   (is_equal(plane.guided_state.target_alt,new_target_alt) ) {  // compare two floats as near-enough
 939                     // no need to process any new packet/s with the same ALT any further, if we are already doing it.
 940                     return MAV_RESULT_ACCEPTED;
 941                 }
 942                 plane.guided_state.target_alt = new_target_alt;
 943                 break;
 944             }
 945             default:
 946                 //  MAV_RESULT_DENIED  means Command is invalid (is supported but has invalid parameters).
 947                 return MAV_RESULT_DENIED;
 948         }
 949
 950         plane.guided_state.target_alt_frame = packet.frame;
 951         plane.guided_state.last_target_alt = plane.current_loc.alt; // FIXME: Reference frame is not corrected for here
 952         plane.guided_state.target_alt_time_ms = AP_HAL::millis();
 953
 954         if (is_zero(packet.param3)) {
 955             // the user wanted /maximum acceleration, pick a large value as close enough
 956             plane.guided_state.target_alt_accel = 1000.0;
 957         } else {
 958             plane.guided_state.target_alt_accel = fabsf(packet.param3);
 959         }
 960
 961          // assign an acceleration direction
 962         if (plane.guided_state.target_alt < plane.current_loc.alt) {
 963             plane.guided_state.target_alt_accel *= -1.0f;
 964         }
 965         return MAV_RESULT_ACCEPTED;
 966     }
 967
 968      case MAV_CMD_GUIDED_CHANGE_HEADING: {
 969
 970         // command is only valid in guided mode
 971         if (plane.control_mode != &plane.mode_guided) {
 972             return MAV_RESULT_FAILED;
 973         }
 974
 975          // don't accept packets outside of [0-360] degree range
 976         if (packet.param2 < 0.0f || packet.param2 >= 360.0f) {
 977             return MAV_RESULT_DENIED;
 978         }
 979
 980         float new_target_heading = radians(wrap_180(packet.param2));
 981
 982         // course over ground
 983         if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int
 984             plane.guided_state.target_heading_type = GUIDED_HEADING_COG;
 985             plane.prev_WP_loc = plane.current_loc;
 986         // normal vehicle heading
 987         } else if (int(packet.param1) == HEADING_TYPE_HEADING) { // compare as nearest int
 988             plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING;
 989         } else {
 990             //  MAV_RESULT_DENIED  means Command is invalid (is supported but has invalid parameters).
 991             return MAV_RESULT_DENIED;
 992         }
 993
 994         plane.g2.guidedHeading.reset_I();
 995
 996         plane.guided_state.target_heading = new_target_heading;
 997         plane.guided_state.target_heading_accel_limit = MAX(packet.param3, 0.05f);
 998         plane.guided_state.target_heading_time_ms = AP_HAL::millis();
 999         return MAV_RESULT_ACCEPTED;
1000     }
1001 #endif // OFFBOARD_GUIDED == ENABLED
1002
1003
1004   }
1005   // anything else ...
1006   return MAV_RESULT_UNSUPPORTED;
1007
1008 }
1009
1010 MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
1011 {
1012     switch(packet.command) {
1013
1014     case MAV_CMD_DO_AUTOTUNE_ENABLE:
1015         return handle_MAV_CMD_DO_AUTOTUNE_ENABLE(packet);
1016
1017     case MAV_CMD_DO_REPOSITION:
1018         return handle_command_int_do_reposition(packet);
1019
1020     // special 'slew-enabled' guided commands here... for speed,alt, and direction commands
1021     case MAV_CMD_GUIDED_CHANGE_SPEED:
1022     case MAV_CMD_GUIDED_CHANGE_ALTITUDE:
1023     case MAV_CMD_GUIDED_CHANGE_HEADING:
1024         return handle_command_int_guided_slew_commands(packet);
1025
1026 #if AP_SCRIPTING_ENABLED && AP_FOLLOW_ENABLED
1027     case MAV_CMD_DO_FOLLOW:
1028         // param1: sysid of target to follow
1029         if ((packet.param1 > 0) && (packet.param1 <= 255)) {
1030             plane.g2.follow.set_target_sysid((uint8_t)packet.param1);
1031             return MAV_RESULT_ACCEPTED;
1032         }
1033         return MAV_RESULT_DENIED;
1034 #endif
1035
1036 #if AP_ICENGINE_ENABLED
1037     case MAV_CMD_DO_ENGINE_CONTROL:
1038         if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3, (uint32_t)packet.param4)) {
1039             return MAV_RESULT_FAILED;
1040         }
1041         return MAV_RESULT_ACCEPTED;
1042 #endif
1043
1044     case MAV_CMD_DO_CHANGE_SPEED:
1045         return handle_command_DO_CHANGE_SPEED(packet);
1046
1047 #if PARACHUTE == ENABLED
1048     case MAV_CMD_DO_PARACHUTE:
1049         return handle_MAV_CMD_DO_PARACHUTE(packet);
1050 #endif
1051
1052 #if HAL_QUADPLANE_ENABLED
1053     case MAV_CMD_DO_MOTOR_TEST:
1054         return handle_MAV_CMD_DO_MOTOR_TEST(packet);
1055
1056     case MAV_CMD_DO_VTOL_TRANSITION:
1057         return handle_command_DO_VTOL_TRANSITION(packet);
1058
1059     case MAV_CMD_NAV_TAKEOFF:
1060         return handle_command_MAV_CMD_NAV_TAKEOFF(packet);
1061 #endif
1062
1063     case MAV_CMD_DO_GO_AROUND:
1064         return plane.trigger_land_abort(packet.param1) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED;
1065
1066     case MAV_CMD_DO_LAND_START:
1067         // attempt to switch to next DO_LAND_START command in the mission
1068         if (plane.have_position && plane.mission.jump_to_landing_sequence(plane.current_loc)) {
1069             plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
1070             return MAV_RESULT_ACCEPTED;
1071         }
1072         return MAV_RESULT_FAILED;
1073
1074     case MAV_CMD_MISSION_START:
1075         plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
1076         return MAV_RESULT_ACCEPTED;
1077
1078     case MAV_CMD_NAV_LOITER_UNLIM:
1079         plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND);
1080         return MAV_RESULT_ACCEPTED;
1081
1082     case MAV_CMD_NAV_RETURN_TO_LAUNCH:
1083         plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND);
1084         return MAV_RESULT_ACCEPTED;
1085
1086     default:
1087         return GCS_MAVLINK::handle_command_int_packet(packet, msg);
1088     }
1089 }
1090
1091 MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_command_int_t &packet)
1092 {
1093         // if we're in failsafe modes (e.g., RTL, LOITER) or in pilot
1094         // controlled modes (e.g., MANUAL, TRAINING)
1095         // this command should be ignored since it comes in from GCS
1096         // or a companion computer:
1097         if ((!plane.control_mode->is_guided_mode()) &&
1098             (plane.control_mode != &plane.mode_auto)) {
1099             // failed
1100             return MAV_RESULT_FAILED;
1101         }
1102
1103         if (plane.do_change_speed(packet.param1, packet.param2, packet.param3)) {
1104             return MAV_RESULT_ACCEPTED;
1105         }
1106         return MAV_RESULT_FAILED;
1107 }
1108
1109 #if HAL_QUADPLANE_ENABLED
1110 #if AP_MAVLINK_COMMAND_LONG_ENABLED
1111 void GCS_MAVLINK_Plane::convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out)
1112 {
1113     // convert to MAV_FRAME_LOCAL_OFFSET_NED, "NED local tangent frame
1114     // with origin that travels with the vehicle"
1115     out = {};
1116     out.target_system = in.target_system;
1117     out.target_component = in.target_component;
1118     out.frame = MAV_FRAME_LOCAL_OFFSET_NED;
1119     out.command = in.command;
1120     // out.current = 0;
1121     // out.autocontinue = 0;
1122     // out.param1 = in.param1;  // we only use the "z" parameter in this command:
1123     // out.param2 = in.param2;
1124     // out.param3 = in.param3;
1125     // out.param4 = in.param4;
1126     // out.x = 0;  // we don't handle positioning when doing takeoffs
1127     // out.y = 0;
1128     out.z = -in.param7;  // up -> down
1129 }
1130
1131 void GCS_MAVLINK_Plane::convert_COMMAND_LONG_to_COMMAND_INT(const mavlink_command_long_t &in, mavlink_command_int_t &out, MAV_FRAME frame)
1132 {
1133     switch (in.command) {
1134     case MAV_CMD_NAV_TAKEOFF:
1135         convert_MAV_CMD_NAV_TAKEOFF_to_COMMAND_INT(in, out);
1136         return;
1137     }
1138     return GCS_MAVLINK::convert_COMMAND_LONG_to_COMMAND_INT(in, out, frame);
1139 }
1140 #endif  // AP_MAVLINK_COMMAND_LONG_ENABLED
1141
1142 MAV_RESULT GCS_MAVLINK_Plane::handle_command_MAV_CMD_NAV_TAKEOFF(const mavlink_command_int_t &packet)
1143 {
1144     float takeoff_alt = packet.z;
1145     switch (packet.frame) {
1146     case MAV_FRAME_LOCAL_OFFSET_NED:  // "NED local tangent frame with origin that travels with the vehicle"
1147         takeoff_alt = -takeoff_alt;  // down -> up
1148         break;
1149     default:
1150         return MAV_RESULT_DENIED; // "is supported but has invalid parameters"
1151     }
1152     if (!plane.quadplane.available()) {
1153         return MAV_RESULT_FAILED;
1154     }
1155     if (!plane.quadplane.do_user_takeoff(takeoff_alt)) {
1156         return MAV_RESULT_FAILED;
1157     }
1158     return MAV_RESULT_ACCEPTED;
1159 }
1160 #endif
1161
1162 MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_AUTOTUNE_ENABLE(const mavlink_command_int_t &packet)
1163 {
1164         // param1 : enable/disable
1165         plane.autotune_enable(!is_zero(packet.param1));
1166         return MAV_RESULT_ACCEPTED;
1167 }
1168
1169 #if PARACHUTE == ENABLED
1170 MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_PARACHUTE(const mavlink_command_int_t &packet)
1171 {
1172         // configure or release parachute
1173         switch ((uint16_t)packet.param1) {
1174         case PARACHUTE_DISABLE:
1175             plane.parachute.enabled(false);
1176             return MAV_RESULT_ACCEPTED;
1177         case PARACHUTE_ENABLE:
1178             plane.parachute.enabled(true);
1179             return MAV_RESULT_ACCEPTED;
1180         case PARACHUTE_RELEASE:
1181             // treat as a manual release which performs some additional check of altitude
1182             if (plane.parachute.released()) {
1183                 gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute already released");
1184                 return MAV_RESULT_FAILED;
1185             }
1186             if (!plane.parachute.enabled()) {
1187                 gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute not enabled");
1188                 return MAV_RESULT_FAILED;
1189             }
1190             if (!plane.parachute_manual_release()) {
1191                 return MAV_RESULT_FAILED;
1192             }
1193             return MAV_RESULT_ACCEPTED;
1194         default:
1195             break;
1196         }
1197         return MAV_RESULT_FAILED;
1198 }
1199 #endif
1200
1201
1202 #if HAL_QUADPLANE_ENABLED
1203 MAV_RESULT GCS_MAVLINK_Plane::handle_MAV_CMD_DO_MOTOR_TEST(const mavlink_command_int_t &packet)
1204 {
1205         // param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
1206         // param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
1207         // param3 : throttle (range depends upon param2)
1208         // param4 : timeout (in seconds)
1209         // param5 : motor count (number of motors to test in sequence)
1210         return plane.quadplane.mavlink_motor_test_start(chan,
1211                                                         (uint8_t)packet.param1,
1212                                                         (uint8_t)packet.param2,
1213                                                         (uint16_t)packet.param3,
1214                                                         packet.param4,
1215                                                         (uint8_t)packet.x);
1216 }
1217
1218 MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_VTOL_TRANSITION(const mavlink_command_int_t &packet)
1219 {
1220         if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) {
1221             return MAV_RESULT_FAILED;
1222         }
1223         return MAV_RESULT_ACCEPTED;
1224 }
1225 #endif
1226
1227 // this is called on receipt of a MANUAL_CONTROL packet and is
1228 // expected to call manual_override to override RC input on desired
1229 // axes.
1230 void GCS_MAVLINK_Plane::handle_manual_control_axes(const mavlink_manual_control_t &packet, const uint32_t tnow)
1231 {
1232     manual_override(plane.channel_roll, packet.y, 1000, 2000, tnow);
1233     manual_override(plane.channel_pitch, packet.x, 1000, 2000, tnow, true);
1234     manual_override(plane.channel_throttle, packet.z, 0, 1000, tnow);
1235     manual_override(plane.channel_rudder, packet.r, 1000, 2000, tnow);
1236 }
1237
1238 void GCS_MAVLINK_Plane::handle_message(const mavlink_message_t &msg)
1239 {
1240     switch (msg.msgid) {
1241
1242     case MAVLINK_MSG_ID_TERRAIN_DATA:
1243     case MAVLINK_MSG_ID_TERRAIN_CHECK:
1244 #if AP_TERRAIN_AVAILABLE
1245         plane.terrain.handle_data(chan, msg);
1246 #endif
1247         break;
1248
1249     case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
1250         handle_set_attitude_target(msg);
1251         break;
1252
1253     case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
1254         handle_set_position_target_local_ned(msg);
1255         break;
1256
1257     case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
1258         handle_set_position_target_global_int(msg);
1259         break;
1260
1261     default:
1262         GCS_MAVLINK::handle_message(msg);
1263         break;
1264     } // end switch
1265 } // end handle mavlink
1266
1267 void GCS_MAVLINK_Plane::handle_set_attitude_target(const mavlink_message_t &msg)
1268     {
1269         // Only allow companion computer (or other external controller) to
1270         // control attitude in GUIDED mode.  We DON'T want external control
1271         // in e.g., RTL, CICLE. Specifying a single mode for companion
1272         // computer control is more safe (even more so when using
1273         // FENCE_ACTION = 4 for geofence failures).
1274         if (plane.control_mode != &plane.mode_guided) { // don't screw up failsafes
1275             return;
1276         }
1277
1278         mavlink_set_attitude_target_t att_target;
1279         mavlink_msg_set_attitude_target_decode(&msg, &att_target);
1280
1281         // Mappings: If any of these bits are set, the corresponding input should be ignored.
1282         // NOTE, when parsing the bits we invert them for easier interpretation but transport has them inverted
1283         // bit 1: body roll rate
1284         // bit 2: body pitch rate
1285         // bit 3: body yaw rate
1286         // bit 4: unknown
1287         // bit 5: unknown
1288         // bit 6: reserved
1289         // bit 7: throttle
1290         // bit 8: attitude
1291
1292         // if not setting all Quaternion values, use _rate flags to indicate which fields.
1293
1294         // Extract the Euler roll angle from the Quaternion.
1295         Quaternion q(att_target.q[0], att_target.q[1],
1296                 att_target.q[2], att_target.q[3]);
1297
1298         // NOTE: att_target.type_mask is inverted for easier interpretation
1299         att_target.type_mask = att_target.type_mask ^ 0xFF;
1300
1301         uint8_t attitude_mask = att_target.type_mask & 0b10000111; // q plus rpy
1302
1303         uint32_t now = AP_HAL::millis();
1304         if ((attitude_mask & 0b10000001) ||    // partial, including roll
1305                 (attitude_mask == 0b10000000)) { // all angles
1306             plane.guided_state.forced_rpy_cd.x = degrees(q.get_euler_roll()) * 100.0f;
1307
1308             // Update timer for external roll to the nav control
1309             plane.guided_state.last_forced_rpy_ms.x = now;
1310         }
1311
1312         if ((attitude_mask & 0b10000010) ||    // partial, including pitch
1313                 (attitude_mask == 0b10000000)) { // all angles
1314             plane.guided_state.forced_rpy_cd.y = degrees(q.get_euler_pitch()) * 100.0f;
1315
1316             // Update timer for external pitch to the nav control
1317             plane.guided_state.last_forced_rpy_ms.y = now;
1318         }
1319
1320         if ((attitude_mask & 0b10000100) ||    // partial, including yaw
1321                 (attitude_mask == 0b10000000)) { // all angles
1322             plane.guided_state.forced_rpy_cd.z = degrees(q.get_euler_yaw()) * 100.0f;
1323
1324             // Update timer for external yaw to the nav control
1325             plane.guided_state.last_forced_rpy_ms.z = now;
1326         }
1327         if (att_target.type_mask & 0b01000000) { // throttle
1328             plane.guided_state.forced_throttle = att_target.thrust * 100.0f;
1329
1330             // Update timer for external throttle
1331             plane.guided_state.last_forced_throttle_ms = now;
1332         }
1333     }
1334
1335 void GCS_MAVLINK_Plane::handle_set_position_target_local_ned(const mavlink_message_t &msg)
1336     {
1337         // decode packet
1338         mavlink_set_position_target_local_ned_t packet;
1339         mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
1340
1341         // exit if vehicle is not in Guided mode
1342         if (plane.control_mode != &plane.mode_guided) {
1343             return;
1344         }
1345
1346         // only local moves for now
1347         if (packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) {
1348             return;
1349         }
1350
1351         // just do altitude for now
1352         plane.next_WP_loc.alt += -packet.z*100.0;
1353         gcs().send_text(MAV_SEVERITY_INFO, "Change alt to %.1f",
1354                         (double)((plane.next_WP_loc.alt - plane.home.alt)*0.01));
1355     }
1356
1357 void GCS_MAVLINK_Plane::handle_set_position_target_global_int(const mavlink_message_t &msg)
1358     {
1359         // Only want to allow companion computer position control when
1360         // in a certain mode to avoid inadvertently sending these
1361         // kinds of commands when the autopilot is responding to problems
1362         // in modes such as RTL, CIRCLE, etc.  Specifying ONLY one mode
1363         // for companion computer control is more safe (provided
1364         // one uses the FENCE_ACTION = 4 (RTL) for geofence failures).
1365         if (plane.control_mode != &plane.mode_guided) {
1366             //don't screw up failsafes
1367             return;
1368         }
1369
1370         mavlink_set_position_target_global_int_t pos_target;
1371         mavlink_msg_set_position_target_global_int_decode(&msg, &pos_target);
1372         // Unexpectedly, the mask is expecting "ones" for dimensions that should
1373         // be IGNORNED rather than INCLUDED.  See mavlink documentation of the
1374         // SET_POSITION_TARGET_GLOBAL_INT message, type_mask field.
1375         const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3)
1376
1377         bool msg_valid = true;
1378         AP_Mission::Mission_Command cmd = {0};
1379
1380         if (pos_target.type_mask & alt_mask)
1381         {
1382             cmd.content.location.alt = pos_target.alt * 100;
1383             cmd.content.location.relative_alt = false;
1384             cmd.content.location.terrain_alt = false;
1385             switch (pos_target.coordinate_frame)
1386             {
1387                 case MAV_FRAME_GLOBAL:
1388                 case MAV_FRAME_GLOBAL_INT:
1389                     break; //default to MSL altitude
1390                 case MAV_FRAME_GLOBAL_RELATIVE_ALT:
1391                 case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
1392                     cmd.content.location.relative_alt = true;
1393                     break;
1394                 case MAV_FRAME_GLOBAL_TERRAIN_ALT:
1395                 case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
1396                     cmd.content.location.relative_alt = true;
1397                     cmd.content.location.terrain_alt = true;
1398                     break;
1399                 default:
1400                     gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT");
1401                     msg_valid = false;
1402                     break;
1403             }
1404
1405             if (msg_valid) {
1406                 handle_change_alt_request(cmd);
1407             }
1408         } // end if alt_mask
1409     }
1410
1411 MAV_RESULT GCS_MAVLINK_Plane::handle_command_do_set_mission_current(const mavlink_command_int_t &packet)
1412 {
1413     const MAV_RESULT result = GCS_MAVLINK::handle_command_do_set_mission_current(packet);
1414     if (result != MAV_RESULT_ACCEPTED) {
1415         return result;
1416     }
1417
1418     // if you change this you must change handle_mission_set_current
1419     plane.auto_state.next_wp_crosstrack = false;
1420     if (plane.control_mode == &plane.mode_auto && plane.mission.state() == AP_Mission::MISSION_STOPPED) {
1421         plane.mission.resume();
1422     }
1423
1424     return result;
1425 }
1426
1427 #if AP_MAVLINK_MISSION_SET_CURRENT_ENABLED
1428 void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg)
1429 {
1430     // if you change this you must change handle_command_do_set_mission_current
1431     plane.auto_state.next_wp_crosstrack = false;
1432     GCS_MAVLINK::handle_mission_set_current(mission, msg);
1433     if (plane.control_mode == &plane.mode_auto && plane.mission.state() == AP_Mission::MISSION_STOPPED) {
1434         plane.mission.resume();
1435     }
1436 }
1437 #endif
1438
1439 uint64_t GCS_MAVLINK_Plane::capabilities() const
1440 {
1441     return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
1442             MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
1443             MAV_PROTOCOL_CAPABILITY_MISSION_INT |
1444             MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
1445             MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
1446 #if AP_TERRAIN_AVAILABLE
1447             (plane.terrain.enabled() ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0) |
1448 #endif
1449             GCS_MAVLINK::capabilities());
1450 }
1451
1452 #if HAL_HIGH_LATENCY2_ENABLED
1453 int16_t GCS_MAVLINK_Plane::high_latency_target_altitude() const
1454 {
1455     AP_AHRS &ahrs = AP::ahrs();
1456     Location global_position_current;
1457     UNUSED_RESULT(ahrs.get_location(global_position_current));
1458
1459 #if HAL_QUADPLANE_ENABLED
1460     const QuadPlane &quadplane = plane.quadplane;
1461     //return units are m
1462     if (quadplane.show_vtol_view()) {
1463         return (plane.control_mode != &plane.mode_qstabilize) ? 0.01 * (global_position_current.alt + quadplane.pos_control->get_pos_error_z_cm()) : 0;
1464     }
1465 #endif
1466     return 0.01 * (global_position_current.alt + plane.calc_altitude_error_cm());
1467 }
1468
1469 uint8_t GCS_MAVLINK_Plane::high_latency_tgt_heading() const
1470 {
1471     // return units are deg/2
1472 #if HAL_QUADPLANE_ENABLED
1473     const QuadPlane &quadplane = plane.quadplane;
1474     if (quadplane.show_vtol_view()) {
1475         const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
1476         return ((uint16_t)(targets.z * 0.01)) / 2;
1477     }
1478 #endif
1479         const AP_Navigation *nav_controller = plane.nav_controller;
1480         // need to convert -18000->18000 to 0->360/2
1481         return wrap_360_cd(nav_controller->target_bearing_cd() ) / 200;
1482 }
1483
1484 // return units are dm
1485 uint16_t GCS_MAVLINK_Plane::high_latency_tgt_dist() const
1486 {
1487 #if HAL_QUADPLANE_ENABLED
1488     const QuadPlane &quadplane = plane.quadplane;
1489     if (quadplane.show_vtol_view()) {
1490         bool wp_nav_valid = quadplane.using_wp_nav();
1491         return (wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination(), UINT16_MAX) : 0) / 10;
1492     }
1493     #endif
1494
1495     return MIN(plane.auto_state.wp_distance, UINT16_MAX) / 10;
1496 }
1497
1498 uint8_t GCS_MAVLINK_Plane::high_latency_tgt_airspeed() const
1499 {
1500     // return units are m/s*5
1501     return plane.target_airspeed_cm * 0.05;
1502 }
1503
1504 uint8_t GCS_MAVLINK_Plane::high_latency_wind_speed() const
1505 {
1506     Vector3f wind;
1507     wind = AP::ahrs().wind_estimate();
1508
1509     // return units are m/s*5
1510     return MIN(wind.length() * 5, UINT8_MAX);
1511 }
1512
1513 uint8_t GCS_MAVLINK_Plane::high_latency_wind_direction() const
1514 {
1515     const Vector3f wind = AP::ahrs().wind_estimate();
1516
1517     // return units are deg/2
1518     // need to convert -180->180 to 0->360/2
1519     return wrap_360(degrees(atan2f(-wind.y, -wind.x))) / 2;
1520 }
1521 #endif // HAL_HIGH_LATENCY2_ENABLED
1522
1523 MAV_VTOL_STATE GCS_MAVLINK_Plane::vtol_state() const
1524 {
1525 #if !HAL_QUADPLANE_ENABLED
1526     return MAV_VTOL_STATE_UNDEFINED;
1527 #else
1528     if (!plane.quadplane.available()) {
1529         return MAV_VTOL_STATE_UNDEFINED;
1530     }
1531
1532     return plane.quadplane.transition->get_mav_vtol_state();
1533 #endif
1534 };
1535
1536 MAV_LANDED_STATE GCS_MAVLINK_Plane::landed_state() const
1537 {
1538     if (plane.is_flying()) {
1539         // note that Q-modes almost always consider themselves as flying
1540         return MAV_LANDED_STATE_IN_AIR;
1541     }
1542
1543     return MAV_LANDED_STATE_ON_GROUND;
1544 }
1545