2016-05-27 09:53:19 -03:00
|
|
|
#include "GCS_Mavlink.h"
|
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
#include "Plane.h"
|
|
|
|
|
2019-03-01 02:27:53 -04:00
|
|
|
MAV_TYPE GCS_Plane::frame_type() const
|
2018-03-22 06:16:35 -03:00
|
|
|
{
|
|
|
|
return plane.quadplane.get_mav_type();
|
|
|
|
}
|
|
|
|
|
|
|
|
MAV_MODE GCS_MAVLINK_Plane::base_mode() const
|
2011-09-18 03:39:09 -03:00
|
|
|
{
|
2018-04-03 11:35:37 -03:00
|
|
|
uint8_t _base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
2011-10-16 04:01:14 -03:00
|
|
|
|
2011-10-29 22:41:32 -03:00
|
|
|
// work out the base_mode. This value is not very useful
|
2011-10-16 04:01:14 -03:00
|
|
|
// for APM, but we calculate it as best we can so a generic
|
|
|
|
// MAVLink enabled ground station can work out something about
|
|
|
|
// what the MAV is up to. The actual bit values are highly
|
|
|
|
// ambiguous for most of the APM flight modes. In practice, you
|
|
|
|
// only get useful information from the custom_mode, which maps to
|
|
|
|
// the APM flight mode and has a well defined meaning in the
|
|
|
|
// ArduPlane documentation
|
2019-01-15 13:46:13 -04:00
|
|
|
switch (plane.control_mode->mode_number()) {
|
|
|
|
case Mode::Number::MANUAL:
|
|
|
|
case Mode::Number::TRAINING:
|
|
|
|
case Mode::Number::ACRO:
|
|
|
|
case Mode::Number::QACRO:
|
2018-04-03 11:35:37 -03:00
|
|
|
_base_mode = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
2011-10-16 04:01:14 -03:00
|
|
|
break;
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::STABILIZE:
|
|
|
|
case Mode::Number::FLY_BY_WIRE_A:
|
|
|
|
case Mode::Number::AUTOTUNE:
|
|
|
|
case Mode::Number::FLY_BY_WIRE_B:
|
|
|
|
case Mode::Number::QSTABILIZE:
|
|
|
|
case Mode::Number::QHOVER:
|
|
|
|
case Mode::Number::QLOITER:
|
|
|
|
case Mode::Number::QLAND:
|
|
|
|
case Mode::Number::CRUISE:
|
|
|
|
case Mode::Number::QAUTOTUNE:
|
2018-04-03 11:35:37 -03:00
|
|
|
_base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
|
2011-10-16 04:01:14 -03:00
|
|
|
break;
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::AUTO:
|
|
|
|
case Mode::Number::RTL:
|
|
|
|
case Mode::Number::LOITER:
|
2020-09-16 04:46:56 -03:00
|
|
|
case Mode::Number::THERMAL:
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::AVOID_ADSB:
|
|
|
|
case Mode::Number::GUIDED:
|
|
|
|
case Mode::Number::CIRCLE:
|
2019-05-04 06:56:07 -03:00
|
|
|
case Mode::Number::TAKEOFF:
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::QRTL:
|
2018-04-03 11:35:37 -03:00
|
|
|
_base_mode = MAV_MODE_FLAG_GUIDED_ENABLED |
|
|
|
|
MAV_MODE_FLAG_STABILIZE_ENABLED;
|
2011-10-16 04:01:14 -03:00
|
|
|
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
|
|
|
|
// APM does in any mode, as that is defined as "system finds its own goal
|
|
|
|
// positions", which APM does not currently do
|
|
|
|
break;
|
2019-01-15 13:46:13 -04:00
|
|
|
case Mode::Number::INITIALISING:
|
2011-10-16 04:01:14 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2018-03-22 06:16:35 -03:00
|
|
|
if (!plane.training_manual_pitch || !plane.training_manual_roll) {
|
2018-04-03 11:35:37 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
|
2012-12-04 02:32:37 -04:00
|
|
|
}
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode != &plane.mode_manual && plane.control_mode != &plane.mode_initializing) {
|
2011-10-16 04:01:14 -03:00
|
|
|
// stabiliser of some form is enabled
|
2018-04-03 11:35:37 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
|
2011-10-16 04:01:14 -03:00
|
|
|
}
|
|
|
|
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.g.stick_mixing != STICK_MIXING_DISABLED && plane.control_mode != &plane.mode_initializing) {
|
2020-03-31 00:49:40 -03:00
|
|
|
if ((plane.g.stick_mixing != STICK_MIXING_VTOL_YAW) || (plane.control_mode == &plane.mode_auto)) {
|
|
|
|
// all modes except INITIALISING have some form of manual
|
|
|
|
// override if stick mixing is enabled
|
|
|
|
_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
|
|
|
}
|
2011-10-16 04:01:14 -03:00
|
|
|
}
|
|
|
|
|
2015-07-13 03:07:40 -03:00
|
|
|
#if HIL_SUPPORT
|
2018-03-22 06:16:35 -03:00
|
|
|
if (plane.g.hil_mode == 1) {
|
2018-04-03 11:35:37 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
2015-03-13 08:33:48 -03:00
|
|
|
}
|
2015-07-13 03:07:40 -03:00
|
|
|
#endif
|
2011-10-16 04:01:14 -03:00
|
|
|
|
|
|
|
// we are armed if we are not initialising
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode != &plane.mode_initializing && plane.arming.is_armed()) {
|
2018-04-03 11:35:37 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
2011-10-16 04:01:14 -03:00
|
|
|
}
|
|
|
|
|
2011-10-29 22:41:32 -03:00
|
|
|
// indicate we have set a custom mode
|
2018-04-03 11:35:37 -03:00
|
|
|
_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
2011-10-29 22:41:32 -03:00
|
|
|
|
2018-04-03 11:35:37 -03:00
|
|
|
return (MAV_MODE)_base_mode;
|
2018-03-22 06:16:35 -03:00
|
|
|
}
|
|
|
|
|
2019-03-01 02:27:53 -04:00
|
|
|
uint32_t GCS_Plane::custom_mode() const
|
2018-03-22 06:16:35 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
return plane.control_mode->mode_number();
|
2011-09-18 03:39:09 -03:00
|
|
|
}
|
|
|
|
|
2019-11-25 22:46:28 -04:00
|
|
|
MAV_STATE GCS_MAVLINK_Plane::vehicle_system_status() const
|
2018-03-22 06:16:35 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode == &plane.mode_initializing) {
|
2018-03-22 06:16:35 -03:00
|
|
|
return MAV_STATE_CALIBRATING;
|
|
|
|
}
|
|
|
|
if (plane.any_failsafe_triggered()) {
|
|
|
|
return MAV_STATE_CRITICAL;
|
|
|
|
}
|
|
|
|
if (plane.crash_state.is_crashed) {
|
|
|
|
return MAV_STATE_EMERGENCY;
|
|
|
|
}
|
|
|
|
if (plane.is_flying()) {
|
|
|
|
return MAV_STATE_ACTIVE;
|
|
|
|
}
|
|
|
|
|
|
|
|
return MAV_STATE_STANDBY;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2018-05-02 03:51:32 -03:00
|
|
|
void GCS_MAVLINK_Plane::send_attitude() const
|
2011-09-18 03:39:09 -03:00
|
|
|
{
|
2018-05-02 03:51:32 -03:00
|
|
|
const AP_AHRS &ahrs = AP::ahrs();
|
|
|
|
|
2017-02-11 04:12:56 -04:00
|
|
|
float r = ahrs.roll;
|
2018-05-02 03:51:32 -03:00
|
|
|
float p = ahrs.pitch - radians(plane.g.pitch_trim_cd*0.01f);
|
2017-02-11 04:12:56 -04:00
|
|
|
float y = ahrs.yaw;
|
|
|
|
|
2020-09-25 18:16:38 -03:00
|
|
|
if (plane.quadplane.show_vtol_view()) {
|
2018-05-02 03:51:32 -03:00
|
|
|
r = plane.quadplane.ahrs_view->roll;
|
|
|
|
p = plane.quadplane.ahrs_view->pitch;
|
|
|
|
y = plane.quadplane.ahrs_view->yaw;
|
2017-02-11 04:12:56 -04:00
|
|
|
}
|
|
|
|
|
2015-05-22 19:57:00 -03:00
|
|
|
const Vector3f &omega = ahrs.get_gyro();
|
2011-09-18 03:39:09 -03:00
|
|
|
mavlink_msg_attitude_send(
|
|
|
|
chan,
|
2015-05-13 19:05:32 -03:00
|
|
|
millis(),
|
2017-02-11 04:12:56 -04:00
|
|
|
r,
|
|
|
|
p,
|
|
|
|
y,
|
2011-09-18 03:39:09 -03:00
|
|
|
omega.x,
|
|
|
|
omega.y,
|
|
|
|
omega.z);
|
|
|
|
}
|
|
|
|
|
2019-11-24 18:24:02 -04:00
|
|
|
void GCS_MAVLINK_Plane::send_aoa_ssa()
|
2017-04-09 08:16:50 -03:00
|
|
|
{
|
2019-11-24 18:24:02 -04:00
|
|
|
AP_AHRS &ahrs = AP::ahrs();
|
|
|
|
|
2017-04-09 08:16:50 -03:00
|
|
|
mavlink_msg_aoa_ssa_send(
|
|
|
|
chan,
|
|
|
|
micros(),
|
|
|
|
ahrs.getAOA(),
|
|
|
|
ahrs.getSSA());
|
|
|
|
}
|
|
|
|
|
2011-12-15 21:41:11 -04:00
|
|
|
#if GEOFENCE_ENABLED == ENABLED
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::send_fence_status(mavlink_channel_t chan)
|
2011-12-15 21:41:11 -04:00
|
|
|
{
|
|
|
|
geofence_send_status(chan);
|
|
|
|
}
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
2018-05-04 21:10:31 -03:00
|
|
|
void GCS_MAVLINK_Plane::send_nav_controller_output() const
|
2011-09-18 03:39:09 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode == &plane.mode_manual) {
|
2018-05-04 21:10:31 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
const QuadPlane &quadplane = plane.quadplane;
|
2018-07-25 18:32:42 -03:00
|
|
|
if (quadplane.in_vtol_mode()) {
|
|
|
|
const Vector3f &targets = quadplane.attitude_control->get_att_target_euler_cd();
|
|
|
|
bool wp_nav_valid = quadplane.using_wp_nav();
|
|
|
|
|
|
|
|
mavlink_msg_nav_controller_output_send(
|
|
|
|
chan,
|
|
|
|
targets.x * 1.0e-2f,
|
|
|
|
targets.y * 1.0e-2f,
|
|
|
|
targets.z * 1.0e-2f,
|
|
|
|
wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0,
|
|
|
|
wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination(), UINT16_MAX) : 0,
|
2019-01-15 13:46:13 -04:00
|
|
|
(plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_alt_error() * 1.0e-2f : 0,
|
2018-05-04 21:10:31 -03:00
|
|
|
plane.airspeed_error * 100,
|
2018-07-25 18:32:42 -03:00
|
|
|
wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0);
|
|
|
|
} else {
|
2018-05-04 21:10:31 -03:00
|
|
|
const AP_Navigation *nav_controller = plane.nav_controller;
|
2018-07-25 18:32:42 -03:00
|
|
|
mavlink_msg_nav_controller_output_send(
|
|
|
|
chan,
|
2018-05-04 21:10:31 -03:00
|
|
|
plane.nav_roll_cd * 0.01f,
|
|
|
|
plane.nav_pitch_cd * 0.01f,
|
2018-07-25 18:32:42 -03:00
|
|
|
nav_controller->nav_bearing_cd() * 0.01f,
|
|
|
|
nav_controller->target_bearing_cd() * 0.01f,
|
2018-05-04 21:10:31 -03:00
|
|
|
MIN(plane.auto_state.wp_distance, UINT16_MAX),
|
|
|
|
plane.altitude_error_cm * 0.01f,
|
|
|
|
plane.airspeed_error * 100,
|
2018-07-25 18:32:42 -03:00
|
|
|
nav_controller->crosstrack_error());
|
|
|
|
}
|
2011-09-18 03:39:09 -03:00
|
|
|
}
|
|
|
|
|
2018-05-11 08:56:26 -03:00
|
|
|
void GCS_MAVLINK_Plane::send_position_target_global_int()
|
2016-04-14 02:22:13 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode == &plane.mode_manual) {
|
2018-05-11 08:56:26 -03:00
|
|
|
return;
|
|
|
|
}
|
|
|
|
Location &next_WP_loc = plane.next_WP_loc;
|
2016-04-14 02:22:13 -03:00
|
|
|
mavlink_msg_position_target_global_int_send(
|
|
|
|
chan,
|
|
|
|
AP_HAL::millis(), // time_boot_ms
|
2019-07-26 22:38:30 -03:00
|
|
|
MAV_FRAME_GLOBAL, // targets are always global altitude
|
2016-04-14 02:22:13 -03:00
|
|
|
0xFFF8, // ignore everything except the x/y/z components
|
|
|
|
next_WP_loc.lat, // latitude as 1e7
|
|
|
|
next_WP_loc.lng, // longitude as 1e7
|
|
|
|
next_WP_loc.alt * 0.01f, // altitude is sent as a float
|
|
|
|
0.0f, // vx
|
|
|
|
0.0f, // vy
|
|
|
|
0.0f, // vz
|
|
|
|
0.0f, // afx
|
|
|
|
0.0f, // afy
|
|
|
|
0.0f, // afz
|
|
|
|
0.0f, // yaw
|
|
|
|
0.0f); // yaw_rate
|
|
|
|
}
|
|
|
|
|
2013-10-23 08:15:06 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
void Plane::send_servo_out(mavlink_channel_t chan)
|
2011-09-18 03:39:09 -03:00
|
|
|
{
|
|
|
|
// normalized values scaled to -10000 to 10000
|
2011-10-16 04:01:14 -03:00
|
|
|
// This is used for HIL. Do not change without discussing with
|
|
|
|
// HIL maintainers
|
|
|
|
mavlink_msg_rc_channels_scaled_send(
|
|
|
|
chan,
|
2015-05-13 19:05:32 -03:00
|
|
|
millis(),
|
2011-10-16 04:01:14 -03:00
|
|
|
0, // port 0
|
2016-10-22 07:27:57 -03:00
|
|
|
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) / 4500.0f),
|
|
|
|
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) / 4500.0f),
|
|
|
|
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) / 100.0f),
|
|
|
|
10000 * (SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) / 4500.0f),
|
2011-10-16 04:01:14 -03:00
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
|
|
|
0,
|
2018-05-02 07:40:30 -03:00
|
|
|
rssi.read_receiver_rssi_uint8());
|
2011-09-18 03:39:09 -03:00
|
|
|
}
|
|
|
|
|
2018-01-28 18:59:58 -04:00
|
|
|
float GCS_MAVLINK_Plane::vfr_hud_airspeed() const
|
2011-09-18 03:39:09 -03:00
|
|
|
{
|
2018-01-28 18:59:58 -04:00
|
|
|
// airspeed sensors are best. While the AHRS airspeed_estimate
|
|
|
|
// will use an airspeed sensor, that value is constrained by the
|
|
|
|
// ground speed. When reporting we should send the true airspeed
|
|
|
|
// value if possible:
|
2018-05-31 19:31:50 -03:00
|
|
|
if (plane.airspeed.enabled() && plane.airspeed.healthy()) {
|
2018-01-28 18:59:58 -04:00
|
|
|
return plane.airspeed.get_airspeed();
|
|
|
|
}
|
|
|
|
|
|
|
|
// airspeed estimates are OK:
|
2012-08-24 09:03:40 -03:00
|
|
|
float aspeed;
|
2020-01-06 20:45:27 -04:00
|
|
|
if (AP::ahrs().airspeed_estimate(aspeed)) {
|
2018-01-28 18:59:58 -04:00
|
|
|
return aspeed;
|
2012-08-24 09:03:40 -03:00
|
|
|
}
|
2018-01-28 18:59:58 -04:00
|
|
|
|
|
|
|
// lying is worst:
|
|
|
|
return 0;
|
|
|
|
}
|
|
|
|
|
|
|
|
int16_t GCS_MAVLINK_Plane::vfr_hud_throttle() const
|
|
|
|
{
|
2019-10-31 19:35:13 -03:00
|
|
|
return plane.throttle_percentage();
|
2018-01-28 18:59:58 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
float GCS_MAVLINK_Plane::vfr_hud_climbrate() const
|
|
|
|
{
|
2020-09-23 05:16:45 -03:00
|
|
|
#if HAL_SOARING_ENABLED
|
2020-04-05 08:38:19 -03:00
|
|
|
if (plane.g2.soaring_controller.is_active()) {
|
2018-01-28 18:59:58 -04:00
|
|
|
return plane.g2.soaring_controller.get_vario_reading();
|
|
|
|
}
|
2018-07-29 19:57:55 -03:00
|
|
|
#endif
|
2018-01-28 18:59:58 -04:00
|
|
|
return AP::baro().get_climb_rate();
|
2011-09-18 03:39:09 -03:00
|
|
|
}
|
|
|
|
|
2013-11-22 19:47:34 -04:00
|
|
|
/*
|
|
|
|
keep last HIL_STATE message to allow sending SIM_STATE
|
|
|
|
*/
|
2015-07-13 03:07:40 -03:00
|
|
|
#if HIL_SUPPORT
|
2013-11-22 19:47:34 -04:00
|
|
|
static mavlink_hil_state_t last_hil_state;
|
2015-07-13 03:07:40 -03:00
|
|
|
#endif
|
2013-11-22 19:47:34 -04:00
|
|
|
|
2012-03-01 00:23:00 -04:00
|
|
|
// report simulator state
|
2018-01-28 19:02:58 -04:00
|
|
|
void GCS_MAVLINK_Plane::send_simstate() const
|
2012-03-01 00:23:00 -04:00
|
|
|
{
|
2015-05-04 03:18:55 -03:00
|
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
2018-01-28 19:02:58 -04:00
|
|
|
GCS_MAVLINK::send_simstate();
|
2015-07-13 03:07:40 -03:00
|
|
|
#elif HIL_SUPPORT
|
2018-01-28 19:02:58 -04:00
|
|
|
if (plane.g.hil_mode == 1) {
|
2015-03-13 08:33:48 -03:00
|
|
|
mavlink_msg_simstate_send(chan,
|
|
|
|
last_hil_state.roll,
|
|
|
|
last_hil_state.pitch,
|
|
|
|
last_hil_state.yaw,
|
|
|
|
last_hil_state.xacc*0.001f*GRAVITY_MSS,
|
|
|
|
last_hil_state.yacc*0.001f*GRAVITY_MSS,
|
|
|
|
last_hil_state.zacc*0.001f*GRAVITY_MSS,
|
|
|
|
last_hil_state.rollspeed,
|
|
|
|
last_hil_state.pitchspeed,
|
|
|
|
last_hil_state.yawspeed,
|
|
|
|
last_hil_state.lat,
|
|
|
|
last_hil_state.lon);
|
|
|
|
}
|
2012-03-01 00:23:00 -04:00
|
|
|
#endif
|
2012-12-20 07:46:48 -04:00
|
|
|
}
|
2012-03-01 00:23:00 -04:00
|
|
|
|
2019-11-24 18:28:33 -04:00
|
|
|
void GCS_MAVLINK_Plane::send_wind() const
|
2012-08-11 02:57:13 -03:00
|
|
|
{
|
2019-11-24 18:28:33 -04:00
|
|
|
const Vector3f wind = AP::ahrs().wind_estimate();
|
2012-08-11 02:57:13 -03:00
|
|
|
mavlink_msg_wind_send(
|
|
|
|
chan,
|
2013-01-10 14:42:24 -04:00
|
|
|
degrees(atan2f(-wind.y, -wind.x)), // use negative, to give
|
2012-11-18 16:10:26 -04:00
|
|
|
// direction wind is coming from
|
2012-12-18 22:36:00 -04:00
|
|
|
wind.length(),
|
2012-08-11 02:57:13 -03:00
|
|
|
wind.z);
|
|
|
|
}
|
|
|
|
|
2018-03-02 20:46:46 -04:00
|
|
|
// sends a single pid info over the provided channel
|
2019-02-28 19:33:00 -04:00
|
|
|
void GCS_MAVLINK_Plane::send_pid_info(const AP_Logger::PID_Info *pid_info,
|
2018-03-02 20:46:46 -04:00
|
|
|
const uint8_t axis, const float achieved)
|
|
|
|
{
|
|
|
|
if (pid_info == nullptr) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (!HAVE_PAYLOAD_SPACE(chan, PID_TUNING)) {
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
mavlink_msg_pid_tuning_send(chan, axis,
|
2019-06-27 06:31:52 -03:00
|
|
|
pid_info->target,
|
2018-03-02 20:46:46 -04:00
|
|
|
achieved,
|
|
|
|
pid_info->FF,
|
|
|
|
pid_info->P,
|
|
|
|
pid_info->I,
|
|
|
|
pid_info->D);
|
|
|
|
}
|
|
|
|
|
2015-05-22 19:57:00 -03:00
|
|
|
/*
|
|
|
|
send PID tuning message
|
|
|
|
*/
|
2019-02-28 19:33:00 -04:00
|
|
|
void GCS_MAVLINK_Plane::send_pid_tuning()
|
2015-05-22 19:57:00 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode == &plane.mode_manual) {
|
2019-02-28 19:33:00 -04:00
|
|
|
// no PIDs should be used in manual
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
|
|
|
|
const Parameters &g = plane.g;
|
|
|
|
|
2019-01-18 00:23:42 -04:00
|
|
|
const AP_Logger::PID_Info *pid_info;
|
2018-03-02 20:46:46 -04:00
|
|
|
if (g.gcs_pid_mask & TUNING_BITS_ROLL) {
|
2019-02-28 19:33:00 -04:00
|
|
|
if (plane.quadplane.in_vtol_mode()) {
|
|
|
|
pid_info = &plane.quadplane.attitude_control->get_rate_roll_pid().get_pid_info();
|
2016-02-28 23:10:20 -04:00
|
|
|
} else {
|
2019-02-28 19:33:00 -04:00
|
|
|
pid_info = &plane.rollController.get_pid_info();
|
2016-02-28 23:10:20 -04:00
|
|
|
}
|
2020-05-18 09:11:34 -03:00
|
|
|
send_pid_info(pid_info, PID_TUNING_ROLL, pid_info->actual);
|
2015-05-22 19:57:00 -03:00
|
|
|
}
|
2018-03-02 20:46:46 -04:00
|
|
|
if (g.gcs_pid_mask & TUNING_BITS_PITCH) {
|
2019-02-28 19:33:00 -04:00
|
|
|
if (plane.quadplane.in_vtol_mode()) {
|
|
|
|
pid_info = &plane.quadplane.attitude_control->get_rate_pitch_pid().get_pid_info();
|
2016-02-28 23:10:20 -04:00
|
|
|
} else {
|
2019-02-28 19:33:00 -04:00
|
|
|
pid_info = &plane.pitchController.get_pid_info();
|
2016-02-28 23:10:20 -04:00
|
|
|
}
|
2020-05-18 09:11:34 -03:00
|
|
|
send_pid_info(pid_info, PID_TUNING_PITCH, pid_info->actual);
|
2015-05-22 19:57:00 -03:00
|
|
|
}
|
2018-03-02 20:46:46 -04:00
|
|
|
if (g.gcs_pid_mask & TUNING_BITS_YAW) {
|
2019-02-28 19:33:00 -04:00
|
|
|
if (plane.quadplane.in_vtol_mode()) {
|
|
|
|
pid_info = &plane.quadplane.attitude_control->get_rate_yaw_pid().get_pid_info();
|
2016-02-28 23:10:20 -04:00
|
|
|
} else {
|
2019-02-28 19:33:00 -04:00
|
|
|
pid_info = &plane.yawController.get_pid_info();
|
2016-02-28 23:10:20 -04:00
|
|
|
}
|
2020-05-18 09:11:34 -03:00
|
|
|
send_pid_info(pid_info, PID_TUNING_YAW, pid_info->actual);
|
2015-06-22 00:27:40 -03:00
|
|
|
}
|
2018-03-02 20:46:46 -04:00
|
|
|
if (g.gcs_pid_mask & TUNING_BITS_STEER) {
|
2020-05-18 09:11:34 -03:00
|
|
|
pid_info = &plane.steerController.get_pid_info();
|
|
|
|
send_pid_info(pid_info, PID_TUNING_STEER, pid_info->actual);
|
2015-06-08 08:08:34 -03:00
|
|
|
}
|
2019-02-28 19:33:00 -04:00
|
|
|
if ((g.gcs_pid_mask & TUNING_BITS_LAND) && (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND)) {
|
2020-05-18 09:11:34 -03:00
|
|
|
AP_AHRS &ahrs = AP::ahrs();
|
|
|
|
const Vector3f &gyro = ahrs.get_gyro();
|
2019-02-28 19:33:00 -04:00
|
|
|
send_pid_info(plane.landing.get_pid_info(), PID_TUNING_LANDING, degrees(gyro.z));
|
2017-02-14 15:20:47 -04:00
|
|
|
}
|
2019-02-28 19:33:00 -04:00
|
|
|
if (g.gcs_pid_mask & TUNING_BITS_ACCZ && plane.quadplane.in_vtol_mode()) {
|
|
|
|
pid_info = &plane.quadplane.pos_control->get_accel_z_pid().get_pid_info();
|
2020-05-18 09:11:34 -03:00
|
|
|
send_pid_info(pid_info, PID_TUNING_ACCZ, pid_info->actual);
|
2018-11-11 19:22:28 -04:00
|
|
|
}
|
|
|
|
}
|
2015-05-22 19:57:00 -03:00
|
|
|
|
2017-07-12 04:41:28 -03:00
|
|
|
uint8_t GCS_MAVLINK_Plane::sysid_my_gcs() const
|
|
|
|
{
|
|
|
|
return plane.g.sysid_my_gcs;
|
|
|
|
}
|
2018-12-13 19:12:52 -04:00
|
|
|
bool GCS_MAVLINK_Plane::sysid_enforce() const
|
|
|
|
{
|
|
|
|
return plane.g2.sysid_enforce;
|
|
|
|
}
|
2017-07-12 04:41:28 -03:00
|
|
|
|
2016-05-29 21:08:39 -03:00
|
|
|
uint32_t GCS_MAVLINK_Plane::telem_delay() const
|
2012-08-29 19:59:39 -03:00
|
|
|
{
|
2016-05-29 21:08:39 -03:00
|
|
|
return (uint32_t)(plane.g.telem_delay);
|
2012-08-29 19:59:39 -03:00
|
|
|
}
|
|
|
|
|
2011-09-18 03:39:09 -03:00
|
|
|
// try to send a message, return false if it won't fit in the serial tx buffer
|
2016-05-27 09:53:19 -03:00
|
|
|
bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
|
2011-09-18 03:39:09 -03:00
|
|
|
{
|
|
|
|
switch (id) {
|
|
|
|
|
|
|
|
case MSG_SERVO_OUT:
|
2015-07-13 03:07:40 -03:00
|
|
|
#if HIL_SUPPORT
|
2015-05-13 03:09:36 -03:00
|
|
|
if (plane.g.hil_mode == 1) {
|
2015-03-13 08:33:48 -03:00
|
|
|
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.send_servo_out(chan);
|
2015-03-13 08:33:48 -03:00
|
|
|
}
|
2015-07-13 03:07:40 -03:00
|
|
|
#endif
|
2011-09-18 03:39:09 -03:00
|
|
|
break;
|
|
|
|
|
2011-12-15 21:41:11 -04:00
|
|
|
case MSG_FENCE_STATUS:
|
2016-06-01 21:25:32 -03:00
|
|
|
#if GEOFENCE_ENABLED == ENABLED
|
2011-12-15 21:41:11 -04:00
|
|
|
CHECK_PAYLOAD_SIZE(FENCE_STATUS);
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.send_fence_status(chan);
|
2011-12-15 21:41:11 -04:00
|
|
|
#endif
|
2016-06-01 21:25:32 -03:00
|
|
|
break;
|
2011-12-15 21:41:11 -04:00
|
|
|
|
2014-07-21 19:24:00 -03:00
|
|
|
case MSG_TERRAIN:
|
2014-07-24 18:58:39 -03:00
|
|
|
#if AP_TERRAIN_AVAILABLE
|
2014-07-21 19:24:00 -03:00
|
|
|
CHECK_PAYLOAD_SIZE(TERRAIN_REQUEST);
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.terrain.send_request(chan);
|
2014-07-21 19:24:00 -03:00
|
|
|
#endif
|
|
|
|
break;
|
|
|
|
|
2012-08-11 02:57:13 -03:00
|
|
|
case MSG_WIND:
|
|
|
|
CHECK_PAYLOAD_SIZE(WIND);
|
2019-11-24 18:28:33 -04:00
|
|
|
send_wind();
|
2012-08-11 02:57:13 -03:00
|
|
|
break;
|
|
|
|
|
2016-06-15 17:57:48 -03:00
|
|
|
case MSG_ADSB_VEHICLE:
|
2020-09-19 05:38:20 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2016-06-15 17:57:48 -03:00
|
|
|
CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
|
|
|
|
plane.adsb.send_adsb_vehicle(chan);
|
2020-09-19 05:38:20 -03:00
|
|
|
#endif
|
2016-06-15 17:57:48 -03:00
|
|
|
break;
|
2018-05-02 05:11:10 -03:00
|
|
|
|
2017-04-09 08:16:50 -03:00
|
|
|
case MSG_AOA_SSA:
|
|
|
|
CHECK_PAYLOAD_SIZE(AOA_SSA);
|
2019-11-24 18:24:02 -04:00
|
|
|
send_aoa_ssa();
|
2017-06-05 00:22:08 -03:00
|
|
|
break;
|
2017-04-13 03:58:05 -03:00
|
|
|
case MSG_LANDING:
|
|
|
|
plane.landing.send_landing_message(chan);
|
2017-04-09 08:16:50 -03:00
|
|
|
break;
|
2017-07-17 22:01:57 -03:00
|
|
|
default:
|
|
|
|
return GCS_MAVLINK::try_send_message(id);
|
2012-08-21 23:19:51 -03:00
|
|
|
}
|
2011-09-18 03:39:09 -03:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2013-02-19 19:08:49 -04:00
|
|
|
/*
|
|
|
|
default stream rates to 1Hz
|
|
|
|
*/
|
2019-06-18 07:37:53 -03:00
|
|
|
const AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
|
2013-04-03 10:58:25 -03:00
|
|
|
// @Param: RAW_SENS
|
|
|
|
// @DisplayName: Raw sensor stream rate
|
|
|
|
// @Description: Raw sensor stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 10
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK_Parameters, streamRates[0], 1),
|
2013-04-03 10:58:25 -03:00
|
|
|
|
|
|
|
// @Param: EXT_STAT
|
|
|
|
// @DisplayName: Extended status stream rate to ground station
|
|
|
|
// @Description: Extended status stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 10
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK_Parameters, streamRates[1], 1),
|
2013-04-03 10:58:25 -03:00
|
|
|
|
|
|
|
// @Param: RC_CHAN
|
|
|
|
// @DisplayName: RC Channel stream rate to ground station
|
|
|
|
// @Description: RC Channel stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 10
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK_Parameters, streamRates[2], 1),
|
2013-04-03 10:58:25 -03:00
|
|
|
|
|
|
|
// @Param: RAW_CTRL
|
|
|
|
// @DisplayName: Raw Control stream rate to ground station
|
|
|
|
// @Description: Raw Control stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 10
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK_Parameters, streamRates[3], 1),
|
2013-04-03 10:58:25 -03:00
|
|
|
|
|
|
|
// @Param: POSITION
|
|
|
|
// @DisplayName: Position stream rate to ground station
|
|
|
|
// @Description: Position stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 10
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("POSITION", 4, GCS_MAVLINK_Parameters, streamRates[4], 1),
|
2013-04-03 10:58:25 -03:00
|
|
|
|
|
|
|
// @Param: EXTRA1
|
|
|
|
// @DisplayName: Extra data type 1 stream rate to ground station
|
|
|
|
// @Description: Extra data type 1 stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 10
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK_Parameters, streamRates[5], 1),
|
2013-04-03 10:58:25 -03:00
|
|
|
|
|
|
|
// @Param: EXTRA2
|
|
|
|
// @DisplayName: Extra data type 2 stream rate to ground station
|
|
|
|
// @Description: Extra data type 2 stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 10
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK_Parameters, streamRates[6], 1),
|
2013-04-03 10:58:25 -03:00
|
|
|
|
|
|
|
// @Param: EXTRA3
|
|
|
|
// @DisplayName: Extra data type 3 stream rate to ground station
|
|
|
|
// @Description: Extra data type 3 stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 10
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK_Parameters, streamRates[7], 1),
|
2013-04-03 10:58:25 -03:00
|
|
|
|
|
|
|
// @Param: PARAMS
|
|
|
|
// @DisplayName: Parameter stream rate to ground station
|
|
|
|
// @Description: Parameter stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 10
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK_Parameters, streamRates[8], 10),
|
2016-06-15 17:57:48 -03:00
|
|
|
|
|
|
|
// @Param: ADSB
|
|
|
|
// @DisplayName: ADSB stream rate to ground station
|
|
|
|
// @Description: ADSB stream rate to ground station
|
|
|
|
// @Units: Hz
|
|
|
|
// @Range: 0 50
|
|
|
|
// @Increment: 1
|
|
|
|
// @User: Advanced
|
2019-06-18 07:37:53 -03:00
|
|
|
AP_GROUPINFO("ADSB", 9, GCS_MAVLINK_Parameters, streamRates[9], 5),
|
2012-02-12 04:20:56 -04:00
|
|
|
AP_GROUPEND
|
|
|
|
};
|
|
|
|
|
2018-05-21 01:06:49 -03:00
|
|
|
static const ap_message STREAM_RAW_SENSORS_msgs[] = {
|
2018-12-06 17:59:34 -04:00
|
|
|
MSG_RAW_IMU,
|
|
|
|
MSG_SCALED_IMU2,
|
|
|
|
MSG_SCALED_IMU3,
|
2018-12-07 02:29:40 -04:00
|
|
|
MSG_SCALED_PRESSURE,
|
|
|
|
MSG_SCALED_PRESSURE2,
|
|
|
|
MSG_SCALED_PRESSURE3,
|
2018-12-04 01:54:21 -04:00
|
|
|
MSG_SENSOR_OFFSETS
|
2017-08-21 02:53:45 -03:00
|
|
|
};
|
2018-05-21 01:06:49 -03:00
|
|
|
static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
|
2018-12-17 22:08:36 -04:00
|
|
|
MSG_SYS_STATUS,
|
|
|
|
MSG_POWER_STATUS,
|
2018-12-04 01:18:39 -04:00
|
|
|
MSG_MEMINFO,
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_CURRENT_WAYPOINT,
|
|
|
|
MSG_GPS_RAW,
|
|
|
|
MSG_GPS_RTK,
|
|
|
|
MSG_GPS2_RAW,
|
|
|
|
MSG_GPS2_RTK,
|
|
|
|
MSG_NAV_CONTROLLER_OUTPUT,
|
|
|
|
MSG_FENCE_STATUS,
|
|
|
|
MSG_POSITION_TARGET_GLOBAL_INT,
|
|
|
|
};
|
2018-05-21 01:06:49 -03:00
|
|
|
static const ap_message STREAM_POSITION_msgs[] = {
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_LOCATION,
|
|
|
|
MSG_LOCAL_POSITION
|
|
|
|
};
|
2018-05-21 01:06:49 -03:00
|
|
|
static const ap_message STREAM_RAW_CONTROLLER_msgs[] = {
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_SERVO_OUT,
|
|
|
|
};
|
2018-05-21 01:06:49 -03:00
|
|
|
static const ap_message STREAM_RC_CHANNELS_msgs[] = {
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_SERVO_OUTPUT_RAW,
|
2019-07-06 20:02:40 -03:00
|
|
|
MSG_RC_CHANNELS,
|
|
|
|
MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection
|
2017-08-21 02:53:45 -03:00
|
|
|
};
|
2018-05-21 01:06:49 -03:00
|
|
|
static const ap_message STREAM_EXTRA1_msgs[] = {
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_ATTITUDE,
|
2018-12-18 16:23:19 -04:00
|
|
|
MSG_SIMSTATE,
|
|
|
|
MSG_AHRS2,
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_RPM,
|
|
|
|
MSG_AOA_SSA,
|
|
|
|
MSG_PID_TUNING,
|
2018-06-20 20:51:06 -03:00
|
|
|
MSG_LANDING,
|
|
|
|
MSG_ESC_TELEMETRY,
|
2019-11-09 04:55:05 -04:00
|
|
|
MSG_EFI_STATUS,
|
2017-08-21 02:53:45 -03:00
|
|
|
};
|
2018-05-21 01:06:49 -03:00
|
|
|
static const ap_message STREAM_EXTRA2_msgs[] = {
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_VFR_HUD
|
|
|
|
};
|
2018-05-21 01:06:49 -03:00
|
|
|
static const ap_message STREAM_EXTRA3_msgs[] = {
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_AHRS,
|
|
|
|
MSG_HWSTATUS,
|
|
|
|
MSG_WIND,
|
|
|
|
MSG_RANGEFINDER,
|
2018-12-18 06:47:33 -04:00
|
|
|
MSG_DISTANCE_SENSOR,
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_SYSTEM_TIME,
|
2014-07-24 18:58:39 -03:00
|
|
|
#if AP_TERRAIN_AVAILABLE
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_TERRAIN,
|
2014-07-21 19:24:00 -03:00
|
|
|
#endif
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_BATTERY2,
|
|
|
|
MSG_BATTERY_STATUS,
|
|
|
|
MSG_MOUNT_STATUS,
|
|
|
|
MSG_OPTICAL_FLOW,
|
|
|
|
MSG_GIMBAL_REPORT,
|
|
|
|
MSG_MAG_CAL_REPORT,
|
|
|
|
MSG_MAG_CAL_PROGRESS,
|
|
|
|
MSG_EKF_STATUS_REPORT,
|
|
|
|
MSG_VIBRATION,
|
|
|
|
};
|
2018-05-22 22:23:42 -03:00
|
|
|
static const ap_message STREAM_PARAMS_msgs[] = {
|
|
|
|
MSG_NEXT_PARAM
|
|
|
|
};
|
2018-05-21 01:06:49 -03:00
|
|
|
static const ap_message STREAM_ADSB_msgs[] = {
|
2017-08-21 02:53:45 -03:00
|
|
|
MSG_ADSB_VEHICLE
|
|
|
|
};
|
2016-06-15 17:57:48 -03:00
|
|
|
|
2017-08-21 02:53:45 -03:00
|
|
|
const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
|
|
|
|
MAV_STREAM_ENTRY(STREAM_RAW_SENSORS),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_POSITION),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_RAW_CONTROLLER),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_RC_CHANNELS),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_EXTRA1),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_EXTRA2),
|
|
|
|
MAV_STREAM_ENTRY(STREAM_EXTRA3),
|
2018-05-22 22:23:42 -03:00
|
|
|
MAV_STREAM_ENTRY(STREAM_PARAMS),
|
2017-08-21 02:53:45 -03:00
|
|
|
MAV_STREAM_ENTRY(STREAM_ADSB),
|
|
|
|
MAV_STREAM_TERMINATOR // must have this at end of stream_entries
|
|
|
|
};
|
2016-06-15 17:57:48 -03:00
|
|
|
|
2017-08-21 02:53:45 -03:00
|
|
|
bool GCS_MAVLINK_Plane::in_hil_mode() const
|
|
|
|
{
|
|
|
|
#if HIL_SUPPORT
|
|
|
|
return plane.g.hil_mode == 1;
|
|
|
|
#endif
|
|
|
|
return false;
|
2011-09-08 22:29:39 -03:00
|
|
|
}
|
|
|
|
|
2014-03-18 19:59:51 -03:00
|
|
|
/*
|
|
|
|
handle a request to switch to guided mode. This happens via a
|
|
|
|
callback from handle_mission_item()
|
|
|
|
*/
|
2016-05-27 09:53:19 -03:00
|
|
|
bool GCS_MAVLINK_Plane::handle_guided_request(AP_Mission::Mission_Command &cmd)
|
2011-09-08 22:29:39 -03:00
|
|
|
{
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode != &plane.mode_guided) {
|
2015-05-10 01:28:02 -03:00
|
|
|
// only accept position updates when in GUIDED mode
|
2016-04-26 07:17:02 -03:00
|
|
|
return false;
|
2015-05-10 01:28:02 -03:00
|
|
|
}
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.guided_WP_loc = cmd.content.location;
|
2014-03-18 19:59:51 -03:00
|
|
|
|
|
|
|
// add home alt if needed
|
2019-01-02 00:46:09 -04:00
|
|
|
if (plane.guided_WP_loc.relative_alt) {
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.guided_WP_loc.alt += plane.home.alt;
|
2019-01-02 00:46:09 -04:00
|
|
|
plane.guided_WP_loc.relative_alt = 0;
|
2014-03-18 19:59:51 -03:00
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.set_guided_WP();
|
2016-04-26 07:17:02 -03:00
|
|
|
return true;
|
2014-03-18 19:59:51 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
|
|
handle a request to change current WP altitude. This happens via a
|
|
|
|
callback from handle_mission_item()
|
|
|
|
*/
|
2016-05-27 09:53:19 -03:00
|
|
|
void GCS_MAVLINK_Plane::handle_change_alt_request(AP_Mission::Mission_Command &cmd)
|
2014-03-18 19:59:51 -03:00
|
|
|
{
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.next_WP_loc.alt = cmd.content.location.alt;
|
2019-01-02 00:46:09 -04:00
|
|
|
if (cmd.content.location.relative_alt) {
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.next_WP_loc.alt += plane.home.alt;
|
2014-03-18 19:59:51 -03:00
|
|
|
}
|
2019-01-02 00:46:09 -04:00
|
|
|
plane.next_WP_loc.relative_alt = false;
|
|
|
|
plane.next_WP_loc.terrain_alt = cmd.content.location.terrain_alt;
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.reset_offset_altitude();
|
2014-03-18 19:59:51 -03:00
|
|
|
}
|
|
|
|
|
2018-03-17 06:26:03 -03:00
|
|
|
|
|
|
|
MAV_RESULT GCS_MAVLINK_Plane::handle_command_preflight_calibration(const mavlink_command_long_t &packet)
|
|
|
|
{
|
|
|
|
plane.in_calibration = true;
|
|
|
|
MAV_RESULT ret = GCS_MAVLINK::handle_command_preflight_calibration(packet);
|
|
|
|
plane.in_calibration = false;
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
}
|
|
|
|
|
2018-03-18 01:22:12 -03:00
|
|
|
MAV_RESULT GCS_MAVLINK_Plane::_handle_command_preflight_calibration(const mavlink_command_long_t &packet)
|
|
|
|
{
|
2018-03-17 06:26:03 -03:00
|
|
|
if (is_equal(packet.param4,1.0f)) {
|
2018-05-25 17:26:36 -03:00
|
|
|
if (plane.trim_radio()) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
} else {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
2018-03-17 06:26:03 -03:00
|
|
|
}
|
|
|
|
|
|
|
|
return GCS_MAVLINK::_handle_command_preflight_calibration(packet);
|
|
|
|
}
|
|
|
|
|
2016-08-12 16:15:04 -03:00
|
|
|
void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
|
2019-04-30 07:22:51 -03:00
|
|
|
const mavlink_message_t &msg)
|
2016-08-12 16:15:04 -03:00
|
|
|
{
|
2020-09-19 05:38:20 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2016-08-12 16:15:04 -03:00
|
|
|
plane.avoidance_adsb.handle_msg(msg);
|
2020-09-19 05:38:20 -03:00
|
|
|
#endif
|
2016-08-12 16:15:04 -03:00
|
|
|
GCS_MAVLINK::packetReceived(status, msg);
|
|
|
|
}
|
|
|
|
|
2018-07-04 00:08:22 -03:00
|
|
|
|
2019-09-12 06:06:42 -03:00
|
|
|
bool GCS_MAVLINK_Plane::set_home_to_current_location(bool _lock)
|
2018-07-05 22:12:54 -03:00
|
|
|
{
|
2018-05-29 20:47:50 -03:00
|
|
|
if (!plane.set_home_persistently(AP::gps().location())) {
|
|
|
|
return false;
|
|
|
|
}
|
2019-09-12 06:06:42 -03:00
|
|
|
if (_lock) {
|
2018-07-05 22:12:54 -03:00
|
|
|
AP::ahrs().lock_home();
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
2019-09-12 06:06:42 -03:00
|
|
|
bool GCS_MAVLINK_Plane::set_home(const Location& loc, bool _lock)
|
2018-07-05 22:12:54 -03:00
|
|
|
{
|
2018-05-29 20:47:50 -03:00
|
|
|
if (!AP::ahrs().set_home(loc)) {
|
|
|
|
return false;
|
|
|
|
}
|
2019-09-12 06:06:42 -03:00
|
|
|
if (_lock) {
|
2018-07-05 22:12:54 -03:00
|
|
|
AP::ahrs().lock_home();
|
|
|
|
}
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_command_int_t &packet)
|
2018-07-04 00:14:29 -03:00
|
|
|
{
|
2019-08-05 05:47:44 -03:00
|
|
|
// sanity check location
|
|
|
|
if (!check_latlng(packet.x, packet.y)) {
|
2020-06-30 20:12:04 -03:00
|
|
|
return MAV_RESULT_DENIED;
|
2019-08-05 05:47:44 -03:00
|
|
|
}
|
2018-07-04 00:14:29 -03:00
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
Location requested_position {};
|
|
|
|
requested_position.lat = packet.x;
|
|
|
|
requested_position.lng = packet.y;
|
2018-07-04 00:14:29 -03:00
|
|
|
|
2020-06-30 20:12:04 -03:00
|
|
|
if (fabsf(packet.z) > LOCATION_ALT_MAX_M) {
|
|
|
|
return MAV_RESULT_DENIED;
|
2019-08-05 05:47:44 -03:00
|
|
|
}
|
2020-06-30 20:12:04 -03:00
|
|
|
Location::AltFrame frame;
|
|
|
|
if (!mavlink_coordinate_frame_to_location_alt_frame((MAV_FRAME)packet.frame, frame)) {
|
|
|
|
return MAV_RESULT_DENIED; // failed as the location is not valid
|
2019-08-05 05:47:44 -03:00
|
|
|
}
|
2020-06-30 20:12:04 -03:00
|
|
|
requested_position.set_alt_cm((int32_t)(packet.z * 100.0f), frame);
|
2018-07-04 00:14:29 -03:00
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
if (is_zero(packet.param4)) {
|
|
|
|
requested_position.loiter_ccw = 0;
|
|
|
|
} else {
|
|
|
|
requested_position.loiter_ccw = 1;
|
|
|
|
}
|
2018-07-04 00:14:29 -03:00
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
if (requested_position.sanitize(plane.current_loc)) {
|
|
|
|
// if the location wasn't already sane don't load it
|
2020-06-30 20:12:04 -03:00
|
|
|
return MAV_RESULT_DENIED;
|
2019-08-05 05:47:44 -03:00
|
|
|
}
|
2018-07-04 00:14:29 -03:00
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
// location is valid load and set
|
|
|
|
if (((int32_t)packet.param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE) ||
|
|
|
|
(plane.control_mode == &plane.mode_guided)) {
|
2019-10-17 00:49:32 -03:00
|
|
|
plane.set_mode(plane.mode_guided, ModeReason::GCS_COMMAND);
|
2019-08-05 05:47:44 -03:00
|
|
|
plane.guided_WP_loc = requested_position;
|
|
|
|
|
|
|
|
// add home alt if needed
|
|
|
|
if (plane.guided_WP_loc.relative_alt) {
|
|
|
|
plane.guided_WP_loc.alt += plane.home.alt;
|
|
|
|
plane.guided_WP_loc.relative_alt = 0;
|
2018-07-04 00:14:29 -03:00
|
|
|
}
|
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
plane.set_guided_WP();
|
2018-07-04 00:14:29 -03:00
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
2018-07-04 00:14:29 -03:00
|
|
|
|
2020-05-16 01:02:01 -03:00
|
|
|
// these are GUIDED mode commands that are RATE or slew enabled, so you can have more powerful control than default controls.
|
|
|
|
MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_guided_slew_commands(const mavlink_command_int_t &packet)
|
|
|
|
{
|
|
|
|
switch(packet.command) {
|
|
|
|
|
|
|
|
#if OFFBOARD_GUIDED == ENABLED
|
|
|
|
case MAV_CMD_GUIDED_CHANGE_SPEED: {
|
|
|
|
// command is only valid in guided mode
|
|
|
|
if (plane.control_mode != &plane.mode_guided) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// only airspeed commands are supported right now...
|
|
|
|
if (int(packet.param1) != SPEED_TYPE_AIRSPEED) { // since SPEED_TYPE is int in range 0-1 and packet.param1 is a *float* this works.
|
|
|
|
return MAV_RESULT_DENIED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// reject airspeeds that are outside of the tuning envelope
|
|
|
|
if (packet.param2 > plane.aparm.airspeed_max || packet.param2 < plane.aparm.airspeed_min) {
|
|
|
|
return MAV_RESULT_DENIED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// no need to process any new packet/s with the
|
|
|
|
// same airspeed any further, if we are already doing it.
|
|
|
|
float new_target_airspeed_cm = packet.param2 * 100;
|
|
|
|
if ( is_equal(new_target_airspeed_cm,plane.guided_state.target_airspeed_cm)) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
plane.guided_state.target_airspeed_cm = new_target_airspeed_cm;
|
|
|
|
plane.guided_state.target_airspeed_time_ms = AP_HAL::millis();
|
|
|
|
|
|
|
|
if (is_zero(packet.param3)) {
|
|
|
|
// the user wanted /maximum acceleration, pick a large value as close enough
|
|
|
|
plane.guided_state.target_airspeed_accel = 1000.0f;
|
|
|
|
} else {
|
|
|
|
plane.guided_state.target_airspeed_accel = fabsf(packet.param3);
|
|
|
|
}
|
|
|
|
|
|
|
|
// assign an acceleration direction
|
|
|
|
if (plane.guided_state.target_airspeed_cm < plane.target_airspeed_cm) {
|
|
|
|
plane.guided_state.target_airspeed_accel *= -1.0f;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
|
|
|
|
case MAV_CMD_GUIDED_CHANGE_ALTITUDE: {
|
|
|
|
// command is only valid in guided
|
|
|
|
if (plane.control_mode != &plane.mode_guided) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// disallow default value of -1 and dangerous value of zero
|
|
|
|
if (is_equal(packet.z, -1.0f) || is_equal(packet.z, 0.0f)){
|
|
|
|
return MAV_RESULT_DENIED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// the requested alt data might be relative or absolute
|
|
|
|
float new_target_alt = packet.z * 100;
|
|
|
|
float new_target_alt_rel = packet.z * 100 + plane.home.alt;
|
|
|
|
|
|
|
|
// only global/relative/terrain frames are supported
|
|
|
|
switch(packet.frame) {
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT: {
|
|
|
|
if (is_equal(plane.guided_state.target_alt,new_target_alt_rel) ) { // compare two floats as near-enough
|
|
|
|
// no need to process any new packet/s with the same ALT any further, if we are already doing it.
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
plane.guided_state.target_alt = new_target_alt_rel;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
case MAV_FRAME_GLOBAL: {
|
|
|
|
if (is_equal(plane.guided_state.target_alt,new_target_alt) ) { // compare two floats as near-enough
|
|
|
|
// no need to process any new packet/s with the same ALT any further, if we are already doing it.
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
plane.guided_state.target_alt = new_target_alt;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
default:
|
|
|
|
// MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters).
|
|
|
|
return MAV_RESULT_DENIED;
|
|
|
|
}
|
|
|
|
|
|
|
|
plane.guided_state.target_alt_frame = packet.frame;
|
|
|
|
plane.guided_state.last_target_alt = plane.current_loc.alt; // FIXME: Reference frame is not corrected for here
|
|
|
|
plane.guided_state.target_alt_time_ms = AP_HAL::millis();
|
|
|
|
|
|
|
|
if (is_zero(packet.param3)) {
|
|
|
|
// the user wanted /maximum acceleration, pick a large value as close enough
|
|
|
|
plane.guided_state.target_alt_accel = 1000.0;
|
|
|
|
} else {
|
|
|
|
plane.guided_state.target_alt_accel = fabsf(packet.param3);
|
|
|
|
}
|
|
|
|
|
|
|
|
// assign an acceleration direction
|
|
|
|
if (plane.guided_state.target_alt < plane.current_loc.alt) {
|
|
|
|
plane.guided_state.target_alt_accel *= -1.0f;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
|
|
|
|
case MAV_CMD_GUIDED_CHANGE_HEADING: {
|
|
|
|
|
|
|
|
// command is only valid in guided mode
|
|
|
|
if (plane.control_mode != &plane.mode_guided) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// don't accept packets outside of [0-360] degree range
|
|
|
|
if (packet.param2 < 0.0f || packet.param2 >= 360.0f) {
|
|
|
|
return MAV_RESULT_DENIED;
|
|
|
|
}
|
|
|
|
|
|
|
|
float new_target_heading = radians(wrap_180(packet.param2));
|
|
|
|
|
|
|
|
// if packet is requesting us to go to the heading we are already going to, we-re already on it.
|
|
|
|
if ( (is_equal(new_target_heading,plane.guided_state.target_heading))) { // compare two floats as near-enough
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
|
|
|
|
// course over ground
|
|
|
|
if ( int(packet.param1) == HEADING_TYPE_COURSE_OVER_GROUND) { // compare as nearest int
|
|
|
|
plane.guided_state.target_heading_type = GUIDED_HEADING_COG;
|
|
|
|
plane.prev_WP_loc = plane.current_loc;
|
|
|
|
// normal vehicle heading
|
|
|
|
} else if (int(packet.param1) == HEADING_TYPE_HEADING) { // compare as nearest int
|
|
|
|
plane.guided_state.target_heading_type = GUIDED_HEADING_HEADING;
|
|
|
|
} else {
|
|
|
|
// MAV_RESULT_DENIED means Command is invalid (is supported but has invalid parameters).
|
|
|
|
return MAV_RESULT_DENIED;
|
|
|
|
}
|
|
|
|
|
|
|
|
plane.g2.guidedHeading.reset_I();
|
|
|
|
|
|
|
|
plane.guided_state.target_heading = new_target_heading;
|
|
|
|
plane.guided_state.target_heading_accel_limit = MAX(packet.param3, 0.05f);
|
|
|
|
plane.guided_state.target_heading_time_ms = AP_HAL::millis();
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
#endif // OFFBOARD_GUIDED == ENABLED
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
// anything else ...
|
|
|
|
return MAV_RESULT_UNSUPPORTED;
|
|
|
|
|
|
|
|
}
|
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_int_t &packet)
|
|
|
|
{
|
2020-05-16 01:02:01 -03:00
|
|
|
|
|
|
|
plane.Log_Write_MavCmdI(packet);
|
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
switch(packet.command) {
|
2018-07-04 00:14:29 -03:00
|
|
|
|
2019-08-05 05:47:44 -03:00
|
|
|
case MAV_CMD_DO_REPOSITION:
|
|
|
|
return handle_command_int_do_reposition(packet);
|
2018-07-04 00:14:29 -03:00
|
|
|
|
2020-05-16 01:02:01 -03:00
|
|
|
// special 'slew-enabled' guided commands here... for speed,alt, and direction commands
|
|
|
|
case MAV_CMD_GUIDED_CHANGE_SPEED:
|
|
|
|
case MAV_CMD_GUIDED_CHANGE_ALTITUDE:
|
|
|
|
case MAV_CMD_GUIDED_CHANGE_HEADING:
|
|
|
|
return handle_command_int_guided_slew_commands(packet);
|
|
|
|
|
2018-07-04 00:14:29 -03:00
|
|
|
default:
|
|
|
|
return GCS_MAVLINK::handle_command_int_packet(packet);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-07-04 00:08:22 -03:00
|
|
|
MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_long_t &packet)
|
|
|
|
{
|
|
|
|
switch(packet.command) {
|
|
|
|
|
2019-01-01 23:25:29 -04:00
|
|
|
case MAV_CMD_DO_CHANGE_SPEED: {
|
2018-07-04 00:08:22 -03:00
|
|
|
// if we're in failsafe modes (e.g., RTL, LOITER) or in pilot
|
|
|
|
// controlled modes (e.g., MANUAL, TRAINING)
|
|
|
|
// this command should be ignored since it comes in from GCS
|
|
|
|
// or a companion computer:
|
2020-09-22 14:00:39 -03:00
|
|
|
if ((!plane.control_mode->is_guided_mode()) &&
|
|
|
|
(plane.control_mode != &plane.mode_auto)) {
|
2018-07-04 00:08:22 -03:00
|
|
|
// failed
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
|
|
|
|
AP_Mission::Mission_Command cmd;
|
2020-10-04 15:20:47 -03:00
|
|
|
if (AP_Mission::mavlink_cmd_long_to_mission_cmd(packet, cmd) != MAV_MISSION_ACCEPTED) {
|
|
|
|
return MAV_RESULT_DENIED;
|
|
|
|
}
|
|
|
|
if (plane.do_change_speed(cmd)) {
|
2018-07-04 00:08:22 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
2019-01-01 23:25:29 -04:00
|
|
|
}
|
2018-07-04 00:08:22 -03:00
|
|
|
|
|
|
|
case MAV_CMD_NAV_LOITER_UNLIM:
|
2019-10-17 00:49:32 -03:00
|
|
|
plane.set_mode(plane.mode_loiter, ModeReason::GCS_COMMAND);
|
2018-07-04 00:08:22 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
|
|
|
case MAV_CMD_NAV_RETURN_TO_LAUNCH:
|
2019-10-17 00:49:32 -03:00
|
|
|
plane.set_mode(plane.mode_rtl, ModeReason::GCS_COMMAND);
|
2018-07-04 00:08:22 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
|
|
|
case MAV_CMD_NAV_TAKEOFF: {
|
|
|
|
// user takeoff only works with quadplane code for now
|
|
|
|
// param7 : altitude [metres]
|
|
|
|
float takeoff_alt = packet.param7;
|
|
|
|
if (plane.quadplane.available() && plane.quadplane.do_user_takeoff(takeoff_alt)) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
|
|
|
|
case MAV_CMD_MISSION_START:
|
2019-10-17 00:49:32 -03:00
|
|
|
plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
|
2018-07-04 00:08:22 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
|
|
|
case MAV_CMD_DO_LAND_START:
|
|
|
|
// attempt to switch to next DO_LAND_START command in the mission
|
|
|
|
if (plane.mission.jump_to_landing_sequence()) {
|
2020-07-16 15:36:11 -03:00
|
|
|
plane.set_mode(plane.mode_auto, ModeReason::GCS_COMMAND);
|
2018-07-04 00:08:22 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
|
|
|
|
case MAV_CMD_DO_GO_AROUND:
|
2018-12-05 19:28:27 -04:00
|
|
|
{
|
|
|
|
uint16_t mission_id = plane.mission.get_current_nav_cmd().id;
|
|
|
|
bool is_in_landing = (plane.flight_stage == AP_Vehicle::FixedWing::FLIGHT_LAND) ||
|
|
|
|
(mission_id == MAV_CMD_NAV_LAND) ||
|
|
|
|
(mission_id == MAV_CMD_NAV_VTOL_LAND);
|
|
|
|
if (is_in_landing) {
|
|
|
|
// fly a user planned abort pattern if available
|
|
|
|
if (plane.mission.jump_to_abort_landing_sequence()) {
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
2018-07-04 00:08:22 -03:00
|
|
|
|
2018-12-05 19:28:27 -04:00
|
|
|
// only fly a fixed wing abort if we aren't doing quadplane stuff, or potentially
|
|
|
|
// shooting a quadplane approach
|
|
|
|
if ((!plane.quadplane.available()) ||
|
|
|
|
((!plane.quadplane.in_vtol_auto()) &&
|
|
|
|
(!(plane.quadplane.options & QuadPlane::OPTION_MISSION_LAND_FW_APPROACH)))) {
|
|
|
|
// Initiate an aborted landing. This will trigger a pitch-up and
|
|
|
|
// climb-out to a safe altitude holding heading then one of the
|
|
|
|
// following actions will occur, check for in this order:
|
|
|
|
// - If MAV_CMD_CONTINUE_AND_CHANGE_ALT is next command in mission,
|
|
|
|
// increment mission index to execute it
|
|
|
|
// - else if DO_LAND_START is available, jump to it
|
|
|
|
// - else decrement the mission index to repeat the landing approach
|
|
|
|
|
|
|
|
if (!is_zero(packet.param1)) {
|
|
|
|
plane.auto_state.takeoff_altitude_rel_cm = packet.param1 * 100;
|
|
|
|
}
|
|
|
|
if (plane.landing.request_go_around()) {
|
|
|
|
plane.auto_state.next_wp_crosstrack = false;
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
}
|
2018-07-04 00:08:22 -03:00
|
|
|
}
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
|
|
|
|
case MAV_CMD_DO_FENCE_ENABLE:
|
|
|
|
if (!plane.geofence_present()) {
|
|
|
|
gcs().send_text(MAV_SEVERITY_NOTICE,"Fence not configured");
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
switch((uint16_t)packet.param1) {
|
|
|
|
case 0:
|
2019-09-16 17:11:07 -03:00
|
|
|
if (! plane.geofence_set_enabled(false)) {
|
2018-07-04 00:08:22 -03:00
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
case 1:
|
2019-09-16 17:11:07 -03:00
|
|
|
if (! plane.geofence_set_enabled(true)) {
|
2018-07-04 00:08:22 -03:00
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
case 2: //disable fence floor only
|
|
|
|
if (! plane.geofence_set_floor_enabled(false)) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
gcs().send_text(MAV_SEVERITY_NOTICE,"Fence floor disabled");
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
|
|
|
|
case MAV_CMD_DO_SET_HOME: {
|
|
|
|
// param1 : use current (1=use current location, 0=use specified location)
|
|
|
|
// param5 : latitude
|
|
|
|
// param6 : longitude
|
|
|
|
// param7 : altitude (absolute)
|
|
|
|
if (is_equal(packet.param1,1.0f)) {
|
2018-05-29 20:47:50 -03:00
|
|
|
if (!plane.set_home_persistently(AP::gps().location())) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
2018-07-04 00:08:22 -03:00
|
|
|
AP::ahrs().lock_home();
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
} else {
|
|
|
|
// ensure param1 is zero
|
|
|
|
if (!is_zero(packet.param1)) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
Location new_home_loc {};
|
|
|
|
new_home_loc.lat = (int32_t)(packet.param5 * 1.0e7f);
|
|
|
|
new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f);
|
|
|
|
new_home_loc.alt = (int32_t)(packet.param7 * 100.0f);
|
2018-05-29 20:47:50 -03:00
|
|
|
if (!set_home(new_home_loc, true)) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
2018-07-04 00:08:22 -03:00
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
case MAV_CMD_DO_AUTOTUNE_ENABLE:
|
|
|
|
// param1 : enable/disable
|
|
|
|
plane.autotune_enable(!is_zero(packet.param1));
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
|
|
|
#if PARACHUTE == ENABLED
|
|
|
|
case MAV_CMD_DO_PARACHUTE:
|
|
|
|
// configure or release parachute
|
|
|
|
switch ((uint16_t)packet.param1) {
|
|
|
|
case PARACHUTE_DISABLE:
|
|
|
|
plane.parachute.enabled(false);
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
case PARACHUTE_ENABLE:
|
|
|
|
plane.parachute.enabled(true);
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
case PARACHUTE_RELEASE:
|
|
|
|
// treat as a manual release which performs some additional check of altitude
|
|
|
|
if (plane.parachute.released()) {
|
|
|
|
gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute already released");
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
if (!plane.parachute.enabled()) {
|
|
|
|
gcs().send_text(MAV_SEVERITY_NOTICE, "Parachute not enabled");
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
if (!plane.parachute_manual_release()) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
default:
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
#endif
|
|
|
|
|
|
|
|
case MAV_CMD_DO_MOTOR_TEST:
|
|
|
|
// param1 : motor sequence number (a number from 1 to max number of motors on the vehicle)
|
|
|
|
// param2 : throttle type (0=throttle percentage, 1=PWM, 2=pilot throttle channel pass-through. See MOTOR_TEST_THROTTLE_TYPE enum)
|
|
|
|
// param3 : throttle (range depends upon param2)
|
|
|
|
// param4 : timeout (in seconds)
|
|
|
|
// param5 : motor count (number of motors to test in sequence)
|
|
|
|
return plane.quadplane.mavlink_motor_test_start(chan,
|
|
|
|
(uint8_t)packet.param1,
|
|
|
|
(uint8_t)packet.param2,
|
|
|
|
(uint16_t)packet.param3,
|
|
|
|
packet.param4,
|
|
|
|
(uint8_t)packet.param5);
|
|
|
|
|
|
|
|
case MAV_CMD_DO_VTOL_TRANSITION:
|
|
|
|
if (!plane.quadplane.handle_do_vtol_transition((enum MAV_VTOL_STATE)packet.param1)) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
|
|
|
case MAV_CMD_DO_ENGINE_CONTROL:
|
|
|
|
if (!plane.g2.ice_control.engine_control(packet.param1, packet.param2, packet.param3)) {
|
|
|
|
return MAV_RESULT_FAILED;
|
|
|
|
}
|
|
|
|
return MAV_RESULT_ACCEPTED;
|
|
|
|
|
|
|
|
default:
|
|
|
|
return GCS_MAVLINK::handle_command_long_packet(packet);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2019-04-30 07:22:51 -03:00
|
|
|
void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg)
|
2014-03-18 19:59:51 -03:00
|
|
|
{
|
2019-04-30 07:22:51 -03:00
|
|
|
switch (msg.msgid) {
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2011-12-15 03:09:29 -04:00
|
|
|
#if GEOFENCE_ENABLED == ENABLED
|
2012-08-21 23:19:51 -03:00
|
|
|
// receive a fence point from GCS and store in EEPROM
|
2011-12-15 03:09:29 -04:00
|
|
|
case MAVLINK_MSG_ID_FENCE_POINT: {
|
|
|
|
mavlink_fence_point_t packet;
|
2019-04-30 07:22:51 -03:00
|
|
|
mavlink_msg_fence_point_decode(&msg, &packet);
|
2015-05-13 03:09:36 -03:00
|
|
|
if (plane.g.fence_action != FENCE_ACTION_NONE) {
|
2015-11-18 15:17:50 -04:00
|
|
|
send_text(MAV_SEVERITY_WARNING,"Fencing must be disabled");
|
2015-05-13 03:09:36 -03:00
|
|
|
} else if (packet.count != plane.g.fence_total) {
|
2015-11-18 15:17:50 -04:00
|
|
|
send_text(MAV_SEVERITY_WARNING,"Bad fence point");
|
2016-06-01 18:49:50 -03:00
|
|
|
} else if (!check_latlng(packet.lat,packet.lng)) {
|
2015-11-18 15:17:50 -04:00
|
|
|
send_text(MAV_SEVERITY_WARNING,"Invalid fence point, lat or lng too large");
|
2011-12-15 03:09:29 -04:00
|
|
|
} else {
|
2017-11-21 19:49:12 -04:00
|
|
|
plane.set_fence_point_with_index(Vector2l(packet.lat*1.0e7f, packet.lng*1.0e7f), packet.idx);
|
2011-12-15 03:09:29 -04:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
// send a fence point to GCS
|
2011-12-15 03:09:29 -04:00
|
|
|
case MAVLINK_MSG_ID_FENCE_FETCH_POINT: {
|
|
|
|
mavlink_fence_fetch_point_t packet;
|
2019-04-30 07:22:51 -03:00
|
|
|
mavlink_msg_fence_fetch_point_decode(&msg, &packet);
|
2015-05-13 03:09:36 -03:00
|
|
|
if (packet.idx >= plane.g.fence_total) {
|
2015-11-18 15:17:50 -04:00
|
|
|
send_text(MAV_SEVERITY_WARNING,"Bad fence point");
|
2011-12-15 03:09:29 -04:00
|
|
|
} else {
|
2015-05-13 03:09:36 -03:00
|
|
|
Vector2l point = plane.get_fence_point_with_index(packet.idx);
|
2019-04-30 07:22:51 -03:00
|
|
|
mavlink_msg_fence_point_send(chan, msg.sysid, msg.compid, packet.idx, plane.g.fence_total,
|
2015-05-02 06:32:27 -03:00
|
|
|
point.x*1.0e-7f, point.y*1.0e-7f);
|
2011-12-15 03:09:29 -04:00
|
|
|
}
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
#endif // GEOFENCE_ENABLED
|
|
|
|
|
2019-02-03 17:44:48 -04:00
|
|
|
|
2017-09-09 18:21:18 -03:00
|
|
|
case MAVLINK_MSG_ID_MANUAL_CONTROL:
|
|
|
|
{
|
2019-04-30 07:22:51 -03:00
|
|
|
if (msg.sysid != plane.g.sysid_my_gcs) {
|
2018-04-03 20:20:13 -03:00
|
|
|
break; // only accept control from our gcs
|
|
|
|
}
|
2017-09-09 18:21:18 -03:00
|
|
|
|
|
|
|
mavlink_manual_control_t packet;
|
2019-04-30 07:22:51 -03:00
|
|
|
mavlink_msg_manual_control_decode(&msg, &packet);
|
2017-09-09 18:21:18 -03:00
|
|
|
|
2018-04-03 20:20:13 -03:00
|
|
|
if (packet.target != plane.g.sysid_this_mav) {
|
|
|
|
break; // only accept messages aimed at us
|
|
|
|
}
|
|
|
|
|
2018-04-26 20:39:00 -03:00
|
|
|
uint32_t tnow = AP_HAL::millis();
|
|
|
|
|
2019-03-26 09:29:59 -03:00
|
|
|
manual_override(plane.channel_roll, packet.y, 1000, 2000, tnow);
|
|
|
|
manual_override(plane.channel_pitch, packet.x, 1000, 2000, tnow, true);
|
|
|
|
manual_override(plane.channel_throttle, packet.z, 0, 1000, tnow);
|
|
|
|
manual_override(plane.channel_rudder, packet.r, 1000, 2000, tnow);
|
2017-09-09 18:21:18 -03:00
|
|
|
|
|
|
|
// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
|
2018-04-26 20:39:00 -03:00
|
|
|
plane.failsafe.last_heartbeat_ms = tnow;
|
2017-09-09 18:21:18 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2011-09-08 22:29:39 -03:00
|
|
|
case MAVLINK_MSG_ID_HEARTBEAT:
|
2012-08-21 23:19:51 -03:00
|
|
|
{
|
2012-12-25 17:46:36 -04:00
|
|
|
// We keep track of the last time we received a heartbeat from
|
|
|
|
// our GCS for failsafe purposes
|
2019-04-30 07:22:51 -03:00
|
|
|
if (msg.sysid != plane.g.sysid_my_gcs) break;
|
2015-11-19 23:04:52 -04:00
|
|
|
plane.failsafe.last_heartbeat_ms = AP_HAL::millis();
|
2012-08-21 23:19:51 -03:00
|
|
|
break;
|
|
|
|
}
|
2011-09-08 22:29:39 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
case MAVLINK_MSG_ID_HIL_STATE:
|
|
|
|
{
|
2015-07-13 03:07:40 -03:00
|
|
|
#if HIL_SUPPORT
|
2015-05-13 03:09:36 -03:00
|
|
|
if (plane.g.hil_mode != 1) {
|
2015-03-13 08:33:48 -03:00
|
|
|
break;
|
|
|
|
}
|
2016-06-01 18:49:50 -03:00
|
|
|
|
2012-08-21 23:19:51 -03:00
|
|
|
mavlink_hil_state_t packet;
|
2019-04-30 07:22:51 -03:00
|
|
|
mavlink_msg_hil_state_decode(&msg, &packet);
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2016-06-01 18:49:50 -03:00
|
|
|
// sanity check location
|
|
|
|
if (!check_latlng(packet.lat, packet.lon)) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2013-11-22 19:47:34 -04:00
|
|
|
last_hil_state = packet;
|
|
|
|
|
2014-03-28 16:52:48 -03:00
|
|
|
// set gps hil sensor
|
2019-03-14 22:44:48 -03:00
|
|
|
const Location loc{packet.lat, packet.lon, packet.alt/10, Location::AltFrame::ABSOLUTE};
|
2014-03-28 16:52:48 -03:00
|
|
|
Vector3f vel(packet.vx, packet.vy, packet.vz);
|
|
|
|
vel *= 0.01f;
|
|
|
|
|
2014-08-10 05:17:12 -03:00
|
|
|
// setup airspeed pressure based on 3D speed, no wind
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.airspeed.setHIL(sq(vel.length()) / 2.0f + 2013);
|
2014-08-10 05:17:12 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.gps.setHIL(0, AP_GPS::GPS_OK_FIX_3D,
|
|
|
|
packet.time_usec/1000,
|
2016-05-04 22:42:32 -03:00
|
|
|
loc, vel, 10, 0);
|
2012-08-21 23:19:51 -03:00
|
|
|
|
|
|
|
// rad/sec
|
|
|
|
Vector3f gyros;
|
|
|
|
gyros.x = packet.rollspeed;
|
|
|
|
gyros.y = packet.pitchspeed;
|
|
|
|
gyros.z = packet.yawspeed;
|
|
|
|
|
|
|
|
// m/s/s
|
|
|
|
Vector3f accels;
|
2015-03-13 08:33:48 -03:00
|
|
|
accels.x = packet.xacc * GRAVITY_MSS*0.001f;
|
|
|
|
accels.y = packet.yacc * GRAVITY_MSS*0.001f;
|
|
|
|
accels.z = packet.zacc * GRAVITY_MSS*0.001f;
|
2012-07-04 21:55:58 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.ins.set_gyro(0, gyros);
|
|
|
|
plane.ins.set_accel(0, accels);
|
2012-08-21 23:19:51 -03:00
|
|
|
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.barometer.setHIL(packet.alt*0.001f);
|
|
|
|
plane.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
|
|
|
|
plane.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
|
2013-11-22 21:37:05 -04:00
|
|
|
|
|
|
|
// cope with DCM getting badly off due to HIL lag
|
2015-05-13 03:09:36 -03:00
|
|
|
if (plane.g.hil_err_limit > 0 &&
|
|
|
|
(fabsf(packet.roll - plane.ahrs.roll) > ToRad(plane.g.hil_err_limit) ||
|
|
|
|
fabsf(packet.pitch - plane.ahrs.pitch) > ToRad(plane.g.hil_err_limit) ||
|
|
|
|
wrap_PI(fabsf(packet.yaw - plane.ahrs.yaw)) > ToRad(plane.g.hil_err_limit))) {
|
|
|
|
plane.ahrs.reset_attitude(packet.roll, packet.pitch, packet.yaw);
|
2013-11-22 21:37:05 -04:00
|
|
|
}
|
2015-07-13 03:07:40 -03:00
|
|
|
#endif
|
2012-08-21 23:19:51 -03:00
|
|
|
break;
|
|
|
|
}
|
2011-10-31 18:55:58 -03:00
|
|
|
|
2012-04-01 21:58:44 -03:00
|
|
|
case MAVLINK_MSG_ID_RADIO:
|
2013-08-24 04:59:02 -03:00
|
|
|
case MAVLINK_MSG_ID_RADIO_STATUS:
|
2012-08-21 23:19:51 -03:00
|
|
|
{
|
2019-03-26 02:13:47 -03:00
|
|
|
handle_radio_status(msg, plane.should_log(MASK_LOG_PM));
|
2012-08-21 23:19:51 -03:00
|
|
|
break;
|
|
|
|
}
|
2012-04-01 21:58:44 -03:00
|
|
|
|
2014-07-21 19:24:00 -03:00
|
|
|
case MAVLINK_MSG_ID_TERRAIN_DATA:
|
2014-07-22 05:08:03 -03:00
|
|
|
case MAVLINK_MSG_ID_TERRAIN_CHECK:
|
2014-07-24 18:58:39 -03:00
|
|
|
#if AP_TERRAIN_AVAILABLE
|
2015-05-13 03:09:36 -03:00
|
|
|
plane.terrain.handle_data(chan, msg);
|
2014-07-21 19:24:00 -03:00
|
|
|
#endif
|
|
|
|
break;
|
2015-02-11 04:52:50 -04:00
|
|
|
|
2016-05-09 11:34:11 -03:00
|
|
|
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
|
|
|
|
{
|
2016-06-30 11:48:37 -03:00
|
|
|
// Only allow companion computer (or other external controller) to
|
|
|
|
// control attitude in GUIDED mode. We DON'T want external control
|
|
|
|
// in e.g., RTL, CICLE. Specifying a single mode for companion
|
|
|
|
// computer control is more safe (even more so when using
|
|
|
|
// FENCE_ACTION = 4 for geofence failures).
|
2020-04-30 03:12:49 -03:00
|
|
|
if (plane.control_mode != &plane.mode_guided) { // don't screw up failsafes
|
2016-05-09 11:34:11 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
mavlink_set_attitude_target_t att_target;
|
2019-04-30 07:22:51 -03:00
|
|
|
mavlink_msg_set_attitude_target_decode(&msg, &att_target);
|
2016-05-09 11:34:11 -03:00
|
|
|
|
2016-06-30 12:50:17 -03:00
|
|
|
// Mappings: If any of these bits are set, the corresponding input should be ignored.
|
|
|
|
// NOTE, when parsing the bits we invert them for easier interpretation but transport has them inverted
|
|
|
|
// bit 1: body roll rate
|
|
|
|
// bit 2: body pitch rate
|
|
|
|
// bit 3: body yaw rate
|
|
|
|
// bit 4: unknown
|
|
|
|
// bit 5: unknown
|
|
|
|
// bit 6: reserved
|
|
|
|
// bit 7: throttle
|
|
|
|
// bit 8: attitude
|
2016-05-09 11:34:11 -03:00
|
|
|
|
2016-06-30 12:50:17 -03:00
|
|
|
// if not setting all Quaternion values, use _rate flags to indicate which fields.
|
|
|
|
|
|
|
|
// Extract the Euler roll angle from the Quaternion.
|
|
|
|
Quaternion q(att_target.q[0], att_target.q[1],
|
|
|
|
att_target.q[2], att_target.q[3]);
|
|
|
|
|
|
|
|
// NOTE: att_target.type_mask is inverted for easier interpretation
|
|
|
|
att_target.type_mask = att_target.type_mask ^ 0xFF;
|
|
|
|
|
|
|
|
uint8_t attitude_mask = att_target.type_mask & 0b10000111; // q plus rpy
|
|
|
|
|
|
|
|
uint32_t now = AP_HAL::millis();
|
|
|
|
if ((attitude_mask & 0b10000001) || // partial, including roll
|
|
|
|
(attitude_mask == 0b10000000)) { // all angles
|
|
|
|
plane.guided_state.forced_rpy_cd.x = degrees(q.get_euler_roll()) * 100.0f;
|
2016-05-09 11:34:11 -03:00
|
|
|
|
2016-06-30 11:48:37 -03:00
|
|
|
// Update timer for external roll to the nav control
|
2016-06-30 12:50:17 -03:00
|
|
|
plane.guided_state.last_forced_rpy_ms.x = now;
|
|
|
|
}
|
|
|
|
|
|
|
|
if ((attitude_mask & 0b10000010) || // partial, including pitch
|
|
|
|
(attitude_mask == 0b10000000)) { // all angles
|
|
|
|
plane.guided_state.forced_rpy_cd.y = degrees(q.get_euler_pitch()) * 100.0f;
|
|
|
|
|
|
|
|
// Update timer for external pitch to the nav control
|
|
|
|
plane.guided_state.last_forced_rpy_ms.y = now;
|
2016-05-09 11:34:11 -03:00
|
|
|
}
|
2016-06-30 12:50:17 -03:00
|
|
|
|
|
|
|
if ((attitude_mask & 0b10000100) || // partial, including yaw
|
|
|
|
(attitude_mask == 0b10000000)) { // all angles
|
|
|
|
plane.guided_state.forced_rpy_cd.z = degrees(q.get_euler_yaw()) * 100.0f;
|
|
|
|
|
|
|
|
// Update timer for external yaw to the nav control
|
|
|
|
plane.guided_state.last_forced_rpy_ms.z = now;
|
|
|
|
}
|
|
|
|
if (att_target.type_mask & 0b01000000) { // throttle
|
|
|
|
plane.guided_state.forced_throttle = att_target.thrust * 100.0f;
|
|
|
|
|
|
|
|
// Update timer for external throttle
|
|
|
|
plane.guided_state.last_forced_throttle_ms = now;
|
|
|
|
}
|
|
|
|
|
2016-05-09 11:34:11 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2015-10-03 00:31:09 -03:00
|
|
|
case MAVLINK_MSG_ID_SET_HOME_POSITION:
|
|
|
|
{
|
|
|
|
mavlink_set_home_position_t packet;
|
2019-04-30 07:22:51 -03:00
|
|
|
mavlink_msg_set_home_position_decode(&msg, &packet);
|
2015-10-03 00:31:09 -03:00
|
|
|
Location new_home_loc {};
|
|
|
|
new_home_loc.lat = packet.latitude;
|
|
|
|
new_home_loc.lng = packet.longitude;
|
2016-04-15 16:28:29 -03:00
|
|
|
new_home_loc.alt = packet.altitude / 10;
|
2018-05-29 20:47:50 -03:00
|
|
|
if (!set_home(new_home_loc, false)) {
|
|
|
|
// silently fails...
|
|
|
|
break;
|
|
|
|
}
|
2015-10-03 00:31:09 -03:00
|
|
|
break;
|
|
|
|
}
|
2015-11-19 14:24:55 -04:00
|
|
|
|
2017-09-11 00:22:18 -03:00
|
|
|
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
|
|
|
|
{
|
|
|
|
// decode packet
|
|
|
|
mavlink_set_position_target_local_ned_t packet;
|
2019-04-30 07:22:51 -03:00
|
|
|
mavlink_msg_set_position_target_local_ned_decode(&msg, &packet);
|
2017-09-11 00:22:18 -03:00
|
|
|
|
|
|
|
// exit if vehicle is not in Guided mode
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode != &plane.mode_guided) {
|
2017-09-11 00:22:18 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// only local moves for now
|
|
|
|
if (packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
// just do altitude for now
|
|
|
|
plane.next_WP_loc.alt += -packet.z*100.0;
|
|
|
|
gcs().send_text(MAV_SEVERITY_INFO, "Change alt to %.1f",
|
2017-09-19 22:02:58 -03:00
|
|
|
(double)((plane.next_WP_loc.alt - plane.home.alt)*0.01));
|
2017-09-11 00:22:18 -03:00
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2016-05-09 11:08:21 -03:00
|
|
|
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
|
|
|
|
{
|
2016-06-07 12:27:40 -03:00
|
|
|
// Only want to allow companion computer position control when
|
2016-05-09 11:08:21 -03:00
|
|
|
// in a certain mode to avoid inadvertently sending these
|
|
|
|
// kinds of commands when the autopilot is responding to problems
|
|
|
|
// in modes such as RTL, CIRCLE, etc. Specifying ONLY one mode
|
|
|
|
// for companion computer control is more safe (provided
|
|
|
|
// one uses the FENCE_ACTION = 4 (RTL) for geofence failures).
|
2020-04-30 03:12:49 -03:00
|
|
|
if (plane.control_mode != &plane.mode_guided) {
|
2016-06-07 12:27:40 -03:00
|
|
|
//don't screw up failsafes
|
2016-05-09 11:08:21 -03:00
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
mavlink_set_position_target_global_int_t pos_target;
|
2019-04-30 07:22:51 -03:00
|
|
|
mavlink_msg_set_position_target_global_int_decode(&msg, &pos_target);
|
2016-06-07 12:27:40 -03:00
|
|
|
// Unexpectedly, the mask is expecting "ones" for dimensions that should
|
|
|
|
// be IGNORNED rather than INCLUDED. See mavlink documentation of the
|
|
|
|
// SET_POSITION_TARGET_GLOBAL_INT message, type_mask field.
|
2016-05-09 11:08:21 -03:00
|
|
|
const uint16_t alt_mask = 0b1111111111111011; // (z mask at bit 3)
|
|
|
|
|
|
|
|
bool msg_valid = true;
|
|
|
|
AP_Mission::Mission_Command cmd = {0};
|
|
|
|
|
|
|
|
if (pos_target.type_mask & alt_mask)
|
|
|
|
{
|
|
|
|
cmd.content.location.alt = pos_target.alt * 100;
|
2019-01-02 00:46:09 -04:00
|
|
|
cmd.content.location.relative_alt = false;
|
|
|
|
cmd.content.location.terrain_alt = false;
|
2016-05-09 11:08:21 -03:00
|
|
|
switch (pos_target.coordinate_frame)
|
|
|
|
{
|
2019-07-26 22:34:46 -03:00
|
|
|
case MAV_FRAME_GLOBAL:
|
2016-05-09 11:08:21 -03:00
|
|
|
case MAV_FRAME_GLOBAL_INT:
|
|
|
|
break; //default to MSL altitude
|
|
|
|
case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
|
2019-01-01 23:25:29 -04:00
|
|
|
cmd.content.location.relative_alt = true;
|
2016-05-09 11:08:21 -03:00
|
|
|
break;
|
|
|
|
case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
|
2019-01-01 23:25:29 -04:00
|
|
|
cmd.content.location.relative_alt = true;
|
2019-01-02 00:46:09 -04:00
|
|
|
cmd.content.location.terrain_alt = true;
|
2016-05-09 11:08:21 -03:00
|
|
|
break;
|
|
|
|
default:
|
2017-07-08 22:15:58 -03:00
|
|
|
gcs().send_text(MAV_SEVERITY_WARNING, "Invalid coord frame in SET_POSTION_TARGET_GLOBAL_INT");
|
2016-05-09 11:08:21 -03:00
|
|
|
msg_valid = false;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (msg_valid) {
|
|
|
|
handle_change_alt_request(cmd);
|
|
|
|
}
|
|
|
|
} // end if alt_mask
|
|
|
|
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
|
2015-11-19 14:24:55 -04:00
|
|
|
case MAVLINK_MSG_ID_ADSB_VEHICLE:
|
2016-08-16 20:19:57 -03:00
|
|
|
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG:
|
|
|
|
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
|
2016-08-16 19:29:44 -03:00
|
|
|
case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT:
|
2020-09-19 05:38:20 -03:00
|
|
|
#if HAL_ADSB_ENABLED
|
2016-08-16 20:19:57 -03:00
|
|
|
plane.adsb.handle_message(chan, msg);
|
2020-09-19 05:38:20 -03:00
|
|
|
#endif
|
2016-07-10 21:21:57 -03:00
|
|
|
break;
|
|
|
|
|
2016-11-06 20:04:15 -04:00
|
|
|
default:
|
|
|
|
handle_common_message(msg);
|
2016-01-20 22:50:49 -04:00
|
|
|
break;
|
2011-09-08 22:29:39 -03:00
|
|
|
} // end switch
|
|
|
|
} // end handle mavlink
|
|
|
|
|
2019-02-03 17:44:48 -04:00
|
|
|
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
|
2019-04-30 07:22:51 -03:00
|
|
|
void GCS_MAVLINK_Plane::handle_rc_channels_override(const mavlink_message_t &msg)
|
2019-02-03 17:44:48 -04:00
|
|
|
{
|
|
|
|
plane.failsafe.last_heartbeat_ms = AP_HAL::millis();
|
|
|
|
GCS_MAVLINK::handle_rc_channels_override(msg);
|
|
|
|
}
|
|
|
|
|
2019-04-30 07:22:51 -03:00
|
|
|
void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, const mavlink_message_t &msg)
|
2017-07-08 00:12:52 -03:00
|
|
|
{
|
2017-11-21 15:41:45 -04:00
|
|
|
plane.auto_state.next_wp_crosstrack = false;
|
2017-07-08 00:12:52 -03:00
|
|
|
GCS_MAVLINK::handle_mission_set_current(mission, msg);
|
2019-01-15 13:46:13 -04:00
|
|
|
if (plane.control_mode == &plane.mode_auto && plane.mission.state() == AP_Mission::MISSION_STOPPED) {
|
2017-07-08 00:12:52 -03:00
|
|
|
plane.mission.resume();
|
|
|
|
}
|
|
|
|
}
|
2017-07-12 21:48:41 -03:00
|
|
|
|
2018-03-28 22:03:57 -03:00
|
|
|
uint64_t GCS_MAVLINK_Plane::capabilities() const
|
|
|
|
{
|
|
|
|
return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
|
|
|
|
MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
|
|
|
|
MAV_PROTOCOL_CAPABILITY_MISSION_INT |
|
|
|
|
MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
|
|
|
|
MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
|
|
|
|
#if AP_TERRAIN_AVAILABLE
|
|
|
|
(plane.terrain.enabled() ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0) |
|
|
|
|
#endif
|
|
|
|
GCS_MAVLINK::capabilities());
|
|
|
|
}
|