2015-05-29 23:12:49 -03:00
# include "Copter.h"
2016-05-27 09:59:22 -03:00
# include "GCS_Mavlink.h"
2011-10-11 06:12:37 -03:00
/*
2012-08-16 21:50:03 -03:00
* ! ! NOTE ! !
*
* the use of NOINLINE separate functions for each message type avoids
* a compiler bug in gcc that would cause it to use far more stack
* space than is needed . Without the NOINLINE we use the sum of the
* stack needed for each message type . Please be careful to follow the
* pattern below when adding any new messages
2011-10-11 06:12:37 -03:00
*/
2019-03-01 02:08:33 -04:00
MAV_TYPE GCS_Copter : : frame_type ( ) const
2011-10-11 06:12:37 -03:00
{
2018-03-22 05:50:24 -03:00
return copter . get_frame_mav_type ( ) ;
}
2014-10-16 06:08:24 -03:00
2018-03-22 05:50:24 -03:00
MAV_MODE GCS_MAVLINK_Copter : : base_mode ( ) const
{
2018-04-03 11:35:32 -03:00
uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED ;
2012-04-24 06:53:24 -03:00
// work out the base_mode. This value is not very useful
// 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
2018-03-22 05:50:24 -03:00
switch ( copter . control_mode ) {
2019-09-07 20:33:56 -03:00
case Mode : : Number : : AUTO :
case Mode : : Number : : RTL :
case Mode : : Number : : LOITER :
case Mode : : Number : : AVOID_ADSB :
case Mode : : Number : : FOLLOW :
case Mode : : Number : : GUIDED :
case Mode : : Number : : CIRCLE :
case Mode : : Number : : POSHOLD :
case Mode : : Number : : BRAKE :
case Mode : : Number : : SMART_RTL :
2018-04-03 11:35:32 -03:00
_base_mode | = MAV_MODE_FLAG_GUIDED_ENABLED ;
2012-04-24 06:53:24 -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 ;
2016-01-25 17:26:20 -04:00
default :
break ;
2012-04-24 06:53:24 -03:00
}
// all modes except INITIALISING have some form of manual
// override if stick mixing is enabled
2018-04-03 11:35:32 -03:00
_base_mode | = MAV_MODE_FLAG_MANUAL_INPUT_ENABLED ;
2012-04-24 06:53:24 -03:00
# if HIL_MODE != HIL_MODE_DISABLED
2018-04-03 11:35:32 -03:00
_base_mode | = MAV_MODE_FLAG_HIL_ENABLED ;
2012-04-24 06:53:24 -03:00
# endif
// we are armed if we are not initialising
2018-03-22 05:50:24 -03:00
if ( copter . motors ! = nullptr & & copter . motors - > armed ( ) ) {
2018-04-03 11:35:32 -03:00
_base_mode | = MAV_MODE_FLAG_SAFETY_ARMED ;
2012-04-24 06:53:24 -03:00
}
// indicate we have set a custom mode
2018-04-03 11:35:32 -03:00
_base_mode | = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED ;
2012-04-24 06:53:24 -03:00
2018-04-03 11:35:32 -03:00
return ( MAV_MODE ) _base_mode ;
2018-03-22 05:50:24 -03:00
}
2019-03-01 02:08:33 -04:00
uint32_t GCS_Copter : : custom_mode ( ) const
2018-03-22 05:50:24 -03:00
{
2019-09-07 20:33:56 -03:00
return ( uint32_t ) copter . control_mode ;
2018-03-22 05:50:24 -03:00
}
2019-11-25 22:46:20 -04:00
MAV_STATE GCS_MAVLINK_Copter : : vehicle_system_status ( ) const
2018-03-22 05:50:24 -03:00
{
// set system as critical if any failsafe have triggered
if ( copter . any_failsafe_triggered ( ) ) {
return MAV_STATE_CRITICAL ;
}
if ( copter . ap . land_complete ) {
return MAV_STATE_STANDBY ;
}
return MAV_STATE_ACTIVE ;
2011-10-11 06:12:37 -03:00
}
2018-03-22 05:50:24 -03:00
2018-05-11 08:59:05 -03:00
void GCS_MAVLINK_Copter : : send_position_target_global_int ( )
{
2019-01-01 22:54:31 -04:00
Location target ;
2018-05-11 08:59:05 -03:00
if ( ! copter . flightmode - > get_wp ( target ) ) {
return ;
}
mavlink_msg_position_target_global_int_send (
chan ,
AP_HAL : : millis ( ) , // time_boot_ms
2019-07-26 22:38:23 -03:00
MAV_FRAME_GLOBAL , // targets are always global altitude
2018-05-11 08:59:05 -03:00
0xFFF8 , // ignore everything except the x/y/z components
target . lat , // latitude as 1e7
target . lng , // longitude as 1e7
target . 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
}
2019-03-28 06:42:26 -03:00
void GCS_MAVLINK_Copter : : send_position_target_local_ned ( )
{
2019-08-21 00:41:25 -03:00
# if MODE_GUIDED_ENABLED == ENABLED
2019-03-28 06:42:26 -03:00
if ( ! copter . flightmode - > in_guided_mode ( ) ) {
return ;
}
const GuidedMode guided_mode = copter . mode_guided . mode ( ) ;
Vector3f target_pos ;
Vector3f target_vel ;
uint16_t type_mask ;
if ( guided_mode = = Guided_WP ) {
type_mask = 0x0FF8 ; // ignore everything except position
target_pos = copter . wp_nav - > get_wp_destination ( ) * 0.01f ; // convert to metres
} else if ( guided_mode = = Guided_Velocity ) {
type_mask = 0x0FC7 ; // ignore everything except velocity
2019-04-18 19:49:09 -03:00
target_vel = copter . flightmode - > get_desired_velocity ( ) * 0.01f ; // convert to m/s
2019-03-28 06:42:26 -03:00
} else {
type_mask = 0x0FC0 ; // ignore everything except position & velocity
target_pos = copter . wp_nav - > get_wp_destination ( ) * 0.01f ;
2019-04-18 19:49:09 -03:00
target_vel = copter . flightmode - > get_desired_velocity ( ) * 0.01f ;
2019-03-28 06:42:26 -03:00
}
mavlink_msg_position_target_local_ned_send (
chan ,
AP_HAL : : millis ( ) , // time boot ms
MAV_FRAME_LOCAL_NED ,
type_mask ,
target_pos . x , // x in metres
target_pos . y , // y in metres
- target_pos . z , // z in metres NED frame
target_vel . x , // vx in m/s
target_vel . y , // vy in m/s
- target_vel . z , // vz in m/s NED frame
0.0f , // afx
0.0f , // afy
0.0f , // afz
0.0f , // yaw
0.0f ) ; // yaw_rate
2019-08-21 00:41:25 -03:00
# endif
2019-03-28 06:42:26 -03:00
}
2018-05-04 21:10:00 -03:00
void GCS_MAVLINK_Copter : : send_nav_controller_output ( ) const
2011-10-11 06:12:37 -03:00
{
2019-02-17 19:46:06 -04:00
if ( ! copter . ap . initialised ) {
return ;
}
2018-05-04 21:10:00 -03:00
const Vector3f & targets = copter . attitude_control - > get_att_target_euler_cd ( ) ;
2019-05-09 23:18:49 -03:00
const Mode * flightmode = copter . flightmode ;
2011-10-11 06:12:37 -03:00
mavlink_msg_nav_controller_output_send (
chan ,
2017-06-19 02:39:56 -03:00
targets . x * 1.0e-2 f ,
targets . y * 1.0e-2 f ,
targets . z * 1.0e-2 f ,
2017-12-05 21:28:32 -04:00
flightmode - > wp_bearing ( ) * 1.0e-2 f ,
MIN ( flightmode - > wp_distance ( ) * 1.0e-2 f , UINT16_MAX ) ,
2018-05-04 21:10:00 -03:00
copter . pos_control - > get_alt_error ( ) * 1.0e-2 f ,
2011-12-31 03:37:24 -04:00
0 ,
2018-05-23 12:34:24 -03:00
flightmode - > crosstrack_error ( ) * 1.0e-2 f ) ;
2011-10-11 06:12:37 -03:00
}
2017-11-20 02:30:39 -04:00
int16_t GCS_MAVLINK_Copter : : vfr_hud_throttle ( ) const
2011-10-11 06:12:37 -03:00
{
2019-07-08 05:24:46 -03:00
if ( copter . motors = = nullptr ) {
return 0 ;
}
2017-11-20 02:30:39 -04:00
return ( int16_t ) ( copter . motors - > get_throttle ( ) * 100 ) ;
2011-10-11 06:12:37 -03:00
}
2015-05-22 21:04:54 -03:00
/*
send PID tuning message
*/
2018-04-05 08:31:57 -03:00
void GCS_MAVLINK_Copter : : send_pid_tuning ( )
2015-05-22 21:04:54 -03:00
{
2018-04-05 08:31:57 -03:00
static const PID_TUNING_AXIS axes [ ] = {
PID_TUNING_ROLL ,
PID_TUNING_PITCH ,
PID_TUNING_YAW ,
PID_TUNING_ACCZ
} ;
for ( uint8_t i = 0 ; i < ARRAY_SIZE ( axes ) ; i + + ) {
if ( ! ( copter . g . gcs_pid_mask & ( 1 < < ( axes [ i ] - 1 ) ) ) ) {
continue ;
2015-05-22 21:04:54 -03:00
}
if ( ! HAVE_PAYLOAD_SPACE ( chan , PID_TUNING ) ) {
return ;
}
2019-04-12 23:49:07 -03:00
const AP_Logger : : PID_Info * pid_info = nullptr ;
2018-04-05 08:31:57 -03:00
switch ( axes [ i ] ) {
case PID_TUNING_ROLL :
2019-04-12 23:49:07 -03:00
pid_info = & copter . attitude_control - > get_rate_roll_pid ( ) . get_pid_info ( ) ;
2018-04-05 08:31:57 -03:00
break ;
case PID_TUNING_PITCH :
2019-04-12 23:49:07 -03:00
pid_info = & copter . attitude_control - > get_rate_pitch_pid ( ) . get_pid_info ( ) ;
2018-04-05 08:31:57 -03:00
break ;
case PID_TUNING_YAW :
2019-04-12 23:49:07 -03:00
pid_info = & copter . attitude_control - > get_rate_yaw_pid ( ) . get_pid_info ( ) ;
2018-04-05 08:31:57 -03:00
break ;
case PID_TUNING_ACCZ :
2019-04-12 23:49:07 -03:00
pid_info = & copter . pos_control - > get_accel_z_pid ( ) . get_pid_info ( ) ;
2018-04-05 08:31:57 -03:00
break ;
default :
continue ;
2015-05-24 03:00:25 -03:00
}
2019-04-12 23:49:07 -03:00
if ( pid_info ! = nullptr ) {
mavlink_msg_pid_tuning_send ( chan ,
axes [ i ] ,
2020-05-17 14:15:59 -03:00
pid_info - > target ,
pid_info - > actual ,
pid_info - > FF ,
pid_info - > P ,
pid_info - > I ,
pid_info - > D ) ;
2019-04-12 23:49:07 -03:00
}
2015-05-22 21:04:54 -03:00
}
}
2020-07-24 05:40:49 -03:00
// send winch status message
void GCS_MAVLINK_Copter : : send_winch_status ( ) const
{
# if WINCH_ENABLED == ENABLED
AP_Winch * winch = AP : : winch ( ) ;
if ( winch = = nullptr ) {
return ;
}
winch - > send_status ( * this ) ;
# endif
}
2017-07-12 04:21:28 -03:00
uint8_t GCS_MAVLINK_Copter : : sysid_my_gcs ( ) const
{
return copter . g . sysid_my_gcs ;
}
2018-12-13 19:12:42 -04:00
bool GCS_MAVLINK_Copter : : sysid_enforce ( ) const
{
return copter . g2 . sysid_enforce ;
}
2017-07-12 04:21:28 -03:00
2016-05-29 20:54:36 -03:00
uint32_t GCS_MAVLINK_Copter : : telem_delay ( ) const
2012-08-29 20:03:01 -03:00
{
2016-05-29 20:54:36 -03:00
return ( uint32_t ) ( copter . g . telem_delay ) ;
2012-08-29 20:03:01 -03:00
}
2019-03-01 07:50:22 -04:00
bool GCS_Copter : : vehicle_initialised ( ) const {
2018-12-17 22:08:11 -04:00
return copter . ap . initialised ;
}
2017-07-17 19:51:21 -03:00
// try to send a message, return false if it wasn't sent
2016-05-27 09:59:22 -03:00
bool GCS_MAVLINK_Copter : : try_send_message ( enum ap_message id )
2011-10-11 06:12:37 -03:00
{
2012-08-16 21:50:03 -03:00
switch ( id ) {
2011-10-11 06:12:37 -03:00
2014-07-22 05:22:36 -03:00
case MSG_TERRAIN :
2015-11-13 23:38:47 -04:00
# if AP_TERRAIN_AVAILABLE && AC_TERRAIN
2014-07-22 05:22:36 -03:00
CHECK_PAYLOAD_SIZE ( TERRAIN_REQUEST ) ;
2015-05-29 23:12:49 -03:00
copter . terrain . send_request ( chan ) ;
2014-07-22 05:22:36 -03:00
# endif
break ;
2013-12-15 20:24:27 -04:00
case MSG_WIND :
2017-01-06 21:06:40 -04:00
case MSG_SERVO_OUT :
2017-04-19 07:05:53 -03:00
case MSG_AOA_SSA :
2017-05-18 15:31:17 -03:00
case MSG_LANDING :
2013-12-15 20:24:27 -04:00
// unused
break ;
2019-06-05 07:47:32 -03:00
case MSG_ADSB_VEHICLE : {
2018-02-16 10:21:55 -04:00
# if ADSB_ENABLED == ENABLED
2016-06-15 21:17:57 -03:00
CHECK_PAYLOAD_SIZE ( ADSB_VEHICLE ) ;
copter . adsb . send_adsb_vehicle ( chan ) ;
2019-06-05 07:47:32 -03:00
# endif
# if AC_OAPATHPLANNER_ENABLED == ENABLED
AP_OADatabase * oadb = AP_OADatabase : : get_singleton ( ) ;
if ( oadb ! = nullptr ) {
CHECK_PAYLOAD_SIZE ( ADSB_VEHICLE ) ;
uint16_t interval_ms = 0 ;
if ( get_ap_message_interval ( id , interval_ms ) ) {
oadb - > send_adsb_vehicle ( chan , interval_ms ) ;
}
}
2018-02-16 10:21:55 -04:00
# endif
2016-06-15 21:17:57 -03:00
break ;
2019-06-05 07:47:32 -03:00
}
2018-05-02 05:11:02 -03:00
2017-07-17 20:54:02 -03:00
default :
return GCS_MAVLINK : : try_send_message ( id ) ;
2012-08-16 21:50:03 -03:00
}
2011-10-11 06:12:37 -03:00
return true ;
}
2019-06-18 07:38:00 -03:00
const AP_Param : : GroupInfo GCS_MAVLINK_Parameters : : var_info [ ] = {
2013-01-02 10:36:48 -04:00
// @Param: RAW_SENS
// @DisplayName: Raw sensor stream rate
2017-01-05 13:40:02 -04:00
// @Description: Stream rate of RAW_IMU, SCALED_IMU2, SCALED_IMU3, SCALED_PRESSURE, SCALED_PRESSURE2, SCALED_PRESSURE3 and SENSOR_OFFSETS to ground station
2013-01-02 10:36:48 -04:00
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
2019-06-18 07:38:00 -03:00
AP_GROUPINFO ( " RAW_SENS " , 0 , GCS_MAVLINK_Parameters , streamRates [ 0 ] , 0 ) ,
2013-01-02 10:36:48 -04:00
// @Param: EXT_STAT
// @DisplayName: Extended status stream rate to ground station
2017-07-03 16:42:04 -03:00
// @Description: Stream rate of SYS_STATUS, POWER_STATUS, MEMINFO, CURRENT_WAYPOINT, GPS_RAW_INT, GPS_RTK (if available), GPS2_RAW (if available), GPS2_RTK (if available), NAV_CONTROLLER_OUTPUT, and FENCE_STATUS to ground station
2013-01-02 10:36:48 -04:00
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
2019-06-18 07:38:00 -03:00
AP_GROUPINFO ( " EXT_STAT " , 1 , GCS_MAVLINK_Parameters , streamRates [ 1 ] , 0 ) ,
2013-01-02 10:36:48 -04:00
// @Param: RC_CHAN
// @DisplayName: RC Channel stream rate to ground station
2016-10-13 21:40:09 -03:00
// @Description: Stream rate of SERVO_OUTPUT_RAW and RC_CHANNELS to ground station
2013-01-02 10:36:48 -04:00
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
2019-06-18 07:38:00 -03:00
AP_GROUPINFO ( " RC_CHAN " , 2 , GCS_MAVLINK_Parameters , streamRates [ 2 ] , 0 ) ,
2013-01-02 10:36:48 -04:00
// @Param: RAW_CTRL
// @DisplayName: Raw Control stream rate to ground station
2014-04-06 06:00:32 -03:00
// @Description: Stream rate of RC_CHANNELS_SCALED (HIL only) to ground station
2013-01-02 10:36:48 -04:00
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
2019-06-18 07:38:00 -03:00
AP_GROUPINFO ( " RAW_CTRL " , 3 , GCS_MAVLINK_Parameters , streamRates [ 3 ] , 0 ) ,
2013-01-02 10:36:48 -04:00
// @Param: POSITION
// @DisplayName: Position stream rate to ground station
2017-01-05 13:40:02 -04:00
// @Description: Stream rate of GLOBAL_POSITION_INT and LOCAL_POSITION_NED to ground station
2013-01-02 10:36:48 -04:00
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
2019-06-18 07:38:00 -03:00
AP_GROUPINFO ( " POSITION " , 4 , GCS_MAVLINK_Parameters , streamRates [ 4 ] , 0 ) ,
2013-01-02 10:36:48 -04:00
// @Param: EXTRA1
// @DisplayName: Extra data type 1 stream rate to ground station
2017-01-05 13:40:02 -04:00
// @Description: Stream rate of ATTITUDE, SIMSTATE (SITL only), AHRS2 and PID_TUNING to ground station
2013-01-02 10:36:48 -04:00
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
2019-06-18 07:38:00 -03:00
AP_GROUPINFO ( " EXTRA1 " , 5 , GCS_MAVLINK_Parameters , streamRates [ 5 ] , 0 ) ,
2013-01-02 10:36:48 -04:00
// @Param: EXTRA2
// @DisplayName: Extra data type 2 stream rate to ground station
2014-04-06 06:00:32 -03:00
// @Description: Stream rate of VFR_HUD to ground station
2013-01-02 10:36:48 -04:00
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
2019-06-18 07:38:00 -03:00
AP_GROUPINFO ( " EXTRA2 " , 6 , GCS_MAVLINK_Parameters , streamRates [ 6 ] , 0 ) ,
2013-01-02 10:36:48 -04:00
// @Param: EXTRA3
// @DisplayName: Extra data type 3 stream rate to ground station
2017-01-05 13:40:02 -04:00
// @Description: Stream rate of AHRS, HWSTATUS, SYSTEM_TIME, RANGEFINDER, DISTANCE_SENSOR, TERRAIN_REQUEST, BATTERY2, MOUNT_STATUS, OPTICAL_FLOW, GIMBAL_REPORT, MAG_CAL_REPORT, MAG_CAL_PROGRESS, EKF_STATUS_REPORT, VIBRATION and RPM to ground station
2013-01-02 10:36:48 -04:00
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
2019-06-18 07:38:00 -03:00
AP_GROUPINFO ( " EXTRA3 " , 7 , GCS_MAVLINK_Parameters , streamRates [ 7 ] , 0 ) ,
2013-01-02 10:36:48 -04:00
// @Param: PARAMS
// @DisplayName: Parameter stream rate to ground station
2014-04-06 06:00:32 -03:00
// @Description: Stream rate of PARAM_VALUE to ground station
2013-01-02 10:36:48 -04:00
// @Units: Hz
// @Range: 0 10
// @Increment: 1
// @User: Advanced
2019-06-18 07:38:00 -03:00
AP_GROUPINFO ( " PARAMS " , 8 , GCS_MAVLINK_Parameters , streamRates [ 8 ] , 0 ) ,
2016-06-15 21:17:57 -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:38:00 -03:00
AP_GROUPINFO ( " ADSB " , 9 , GCS_MAVLINK_Parameters , streamRates [ 9 ] , 0 ) ,
2016-06-15 21:17:57 -03:00
AP_GROUPEND
2012-02-12 07:27:03 -04:00
} ;
2011-05-11 03:10:06 -03:00
2018-05-21 00:47:55 -03:00
static const ap_message STREAM_RAW_SENSORS_msgs [ ] = {
2018-12-04 20:37:02 -04:00
MSG_RAW_IMU ,
MSG_SCALED_IMU2 ,
MSG_SCALED_IMU3 ,
2018-12-07 02:29:35 -04:00
MSG_SCALED_PRESSURE ,
MSG_SCALED_PRESSURE2 ,
MSG_SCALED_PRESSURE3 ,
2018-12-04 01:54:13 -04:00
MSG_SENSOR_OFFSETS
2017-08-20 23:07:25 -03:00
} ;
2018-05-21 00:47:55 -03:00
static const ap_message STREAM_EXTENDED_STATUS_msgs [ ] = {
2018-12-17 22:08:11 -04:00
MSG_SYS_STATUS ,
MSG_POWER_STATUS ,
2018-12-04 01:18:33 -04:00
MSG_MEMINFO ,
2017-08-20 23:07:25 -03:00
MSG_CURRENT_WAYPOINT , // MISSION_CURRENT
MSG_GPS_RAW ,
MSG_GPS_RTK ,
MSG_GPS2_RAW ,
MSG_GPS2_RTK ,
MSG_NAV_CONTROLLER_OUTPUT ,
2018-05-11 08:59:05 -03:00
MSG_FENCE_STATUS ,
MSG_POSITION_TARGET_GLOBAL_INT ,
2017-08-20 23:07:25 -03:00
} ;
2018-05-21 00:47:55 -03:00
static const ap_message STREAM_POSITION_msgs [ ] = {
2017-08-20 23:07:25 -03:00
MSG_LOCATION ,
MSG_LOCAL_POSITION
} ;
2018-05-21 00:47:55 -03:00
static const ap_message STREAM_RC_CHANNELS_msgs [ ] = {
2017-08-20 23:07:25 -03:00
MSG_SERVO_OUTPUT_RAW ,
2019-07-06 20:02:12 -03:00
MSG_RC_CHANNELS ,
MSG_RC_CHANNELS_RAW , // only sent on a mavlink1 connection
2017-08-20 23:07:25 -03:00
} ;
2018-05-21 00:47:55 -03:00
static const ap_message STREAM_EXTRA1_msgs [ ] = {
2017-08-20 23:07:25 -03:00
MSG_ATTITUDE ,
2018-12-18 16:23:13 -04:00
MSG_SIMSTATE ,
MSG_AHRS2 ,
2017-08-20 23:07:25 -03:00
MSG_PID_TUNING // Up to four PID_TUNING messages are sent, depending on GCS_PID_MASK parameter
} ;
2018-05-21 00:47:55 -03:00
static const ap_message STREAM_EXTRA2_msgs [ ] = {
2017-08-20 23:07:25 -03:00
MSG_VFR_HUD
} ;
2018-05-21 00:47:55 -03:00
static const ap_message STREAM_EXTRA3_msgs [ ] = {
2017-08-20 23:07:25 -03:00
MSG_AHRS ,
MSG_HWSTATUS ,
MSG_SYSTEM_TIME ,
MSG_RANGEFINDER ,
2018-12-18 06:47:27 -04:00
MSG_DISTANCE_SENSOR ,
2015-11-13 23:38:47 -04:00
# if AP_TERRAIN_AVAILABLE && AC_TERRAIN
2017-08-20 23:07:25 -03:00
MSG_TERRAIN ,
2014-07-22 05:22:36 -03:00
# endif
2017-08-20 23:07:25 -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-06-20 20:51:06 -03:00
MSG_RPM ,
MSG_ESC_TELEMETRY ,
2020-06-29 02:40:39 -03:00
MSG_GENERATOR_STATUS ,
2020-07-24 05:40:49 -03:00
MSG_WINCH_STATUS ,
2017-08-20 23:07:25 -03:00
} ;
2018-05-22 22:23:35 -03:00
static const ap_message STREAM_PARAMS_msgs [ ] = {
MSG_NEXT_PARAM
} ;
2018-05-21 00:47:55 -03:00
static const ap_message STREAM_ADSB_msgs [ ] = {
2017-08-20 23:07:25 -03:00
MSG_ADSB_VEHICLE
} ;
2012-04-01 22:18:42 -03:00
2017-08-20 23:07:25 -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_RC_CHANNELS ) ,
MAV_STREAM_ENTRY ( STREAM_EXTRA1 ) ,
MAV_STREAM_ENTRY ( STREAM_EXTRA2 ) ,
MAV_STREAM_ENTRY ( STREAM_EXTRA3 ) ,
MAV_STREAM_ENTRY ( STREAM_ADSB ) ,
2018-05-22 22:23:35 -03:00
MAV_STREAM_ENTRY ( STREAM_PARAMS ) ,
2017-08-20 23:07:25 -03:00
MAV_STREAM_TERMINATOR // must have this at end of stream_entries
} ;
2012-04-01 22:18:42 -03:00
2016-05-27 09:59:22 -03:00
bool GCS_MAVLINK_Copter : : handle_guided_request ( AP_Mission : : Mission_Command & cmd )
2014-03-18 20:06:48 -03:00
{
2018-02-21 22:06:07 -04:00
# if MODE_AUTO_ENABLED == ENABLED
2017-12-10 22:50:49 -04:00
return copter . mode_auto . do_guided ( cmd ) ;
2018-02-21 22:06:07 -04:00
# else
return false ;
# endif
2014-03-18 20:06:48 -03:00
}
2016-05-27 09:59:22 -03:00
void GCS_MAVLINK_Copter : : handle_change_alt_request ( AP_Mission : : Mission_Command & cmd )
2014-03-18 20:06:48 -03:00
{
// add home alt if needed
2019-01-01 20:09:02 -04:00
if ( cmd . content . location . relative_alt ) {
2015-05-29 23:12:49 -03:00
cmd . content . location . alt + = copter . ahrs . get_home ( ) . alt ;
2014-03-18 20:06:48 -03:00
}
// To-Do: update target altitude for loiter or waypoint controller depending upon nav mode
}
2016-07-21 09:45:00 -03:00
void GCS_MAVLINK_Copter : : packetReceived ( const mavlink_status_t & status ,
2019-04-30 07:22:50 -03:00
const mavlink_message_t & msg )
2016-07-21 09:45:00 -03:00
{
2018-02-16 10:21:55 -04:00
# if ADSB_ENABLED == ENABLED
2016-09-12 03:23:53 -03:00
if ( copter . g2 . dev_options . get ( ) & DevOptionADSBMAVLink ) {
// optional handling of GLOBAL_POSITION_INT as a MAVLink based avoidance source
copter . avoidance_adsb . handle_msg ( msg ) ;
}
2018-02-16 10:21:55 -04:00
# endif
2018-02-06 02:16:09 -04:00
# if MODE_FOLLOW_ENABLED == ENABLED
2018-01-25 08:36:03 -04:00
// pass message to follow library
copter . g2 . follow . handle_msg ( msg ) ;
2018-02-06 02:16:09 -04:00
# endif
2016-07-21 09:45:00 -03:00
GCS_MAVLINK : : packetReceived ( status , msg ) ;
}
2017-08-19 08:23:28 -03:00
bool GCS_MAVLINK_Copter : : params_ready ( ) const
{
2019-11-06 18:21:27 -04:00
if ( AP_BoardConfig : : in_config_error ( ) ) {
2017-08-19 08:23:28 -03:00
// we may never have parameters "initialised" in this case
return true ;
}
// if we have not yet initialised (including allocating the motors
// object) we drop this request. That prevents the GCS from getting
// a confusing parameter count during bootup
return copter . ap . initialised_params ;
}
void GCS_MAVLINK_Copter : : send_banner ( )
{
GCS_MAVLINK : : send_banner ( ) ;
send_text ( MAV_SEVERITY_INFO , " Frame: %s " , copter . get_frame_string ( ) ) ;
}
2019-02-03 17:42:04 -04:00
// a RC override message is considered to be a 'heartbeat' from the ground station for failsafe purposes
2019-04-30 07:22:50 -03:00
void GCS_MAVLINK_Copter : : handle_rc_channels_override ( const mavlink_message_t & msg )
2019-02-03 17:42:04 -04:00
{
copter . failsafe . last_heartbeat_ms = AP_HAL : : millis ( ) ;
GCS_MAVLINK : : handle_rc_channels_override ( msg ) ;
}
2017-08-19 08:23:28 -03:00
2019-04-30 07:22:50 -03:00
void GCS_MAVLINK_Copter : : handle_command_ack ( const mavlink_message_t & msg )
2018-03-20 21:19:42 -03:00
{
copter . command_ack_counter + + ;
GCS_MAVLINK : : handle_command_ack ( msg ) ;
}
2018-03-17 06:12:57 -03:00
MAV_RESULT GCS_MAVLINK_Copter : : _handle_command_preflight_calibration ( const mavlink_command_long_t & packet )
{
if ( is_equal ( packet . param6 , 1.0f ) ) {
// compassmot calibration
2019-06-21 20:49:52 -03:00
return copter . mavlink_compassmot ( * this ) ;
2018-03-17 06:12:57 -03:00
}
return GCS_MAVLINK : : _handle_command_preflight_calibration ( packet ) ;
}
2018-07-03 22:43:51 -03:00
2018-10-13 19:24:43 -03:00
MAV_RESULT GCS_MAVLINK_Copter : : handle_command_do_set_roi ( const Location & roi_loc )
{
2019-04-08 10:51:25 -03:00
if ( ! roi_loc . check_latlng ( ) ) {
2018-10-13 19:24:43 -03:00
return MAV_RESULT_FAILED ;
}
copter . flightmode - > auto_yaw . set_roi ( roi_loc ) ;
return MAV_RESULT_ACCEPTED ;
}
2019-10-04 06:08:35 -03:00
MAV_RESULT GCS_MAVLINK_Copter : : handle_preflight_reboot ( const mavlink_command_long_t & packet )
{
// reject reboot if user has also specified they want the "Auto" ESC calibration on next reboot
if ( copter . g . esc_calibrate = = ( uint8_t ) Copter : : ESCCalibrationModes : : ESCCAL_AUTO ) {
send_text ( MAV_SEVERITY_CRITICAL , " Reboot rejected, ESC cal on reboot " ) ;
return MAV_RESULT_FAILED ;
}
// call parent
return GCS_MAVLINK : : handle_preflight_reboot ( packet ) ;
}
2019-09-12 06:51:19 -03:00
bool GCS_MAVLINK_Copter : : set_home_to_current_location ( bool _lock ) {
return copter . set_home_to_current_location ( _lock ) ;
2018-07-05 21:56:43 -03:00
}
2019-09-12 06:51:19 -03:00
bool GCS_MAVLINK_Copter : : set_home ( const Location & loc , bool _lock ) {
return copter . set_home ( loc , _lock ) ;
2018-07-05 21:56:43 -03:00
}
2020-06-30 17:48:11 -03:00
MAV_RESULT GCS_MAVLINK_Copter : : handle_command_int_do_reposition ( const mavlink_command_int_t & packet )
{
const bool change_modes = ( ( int32_t ) packet . param2 & MAV_DO_REPOSITION_FLAGS_CHANGE_MODE ) = = MAV_DO_REPOSITION_FLAGS_CHANGE_MODE ;
if ( ! copter . flightmode - > in_guided_mode ( ) & & ! change_modes ) {
return MAV_RESULT_DENIED ;
}
// sanity check location
if ( ! check_latlng ( packet . x , packet . y ) ) {
return MAV_RESULT_DENIED ;
}
Location request_location { } ;
request_location . lat = packet . x ;
request_location . lng = packet . y ;
if ( fabsf ( packet . z ) > LOCATION_ALT_MAX_M ) {
return MAV_RESULT_DENIED ;
}
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
}
request_location . set_alt_cm ( ( int32_t ) ( packet . z * 100.0f ) , frame ) ;
if ( request_location . sanitize ( copter . current_loc ) ) {
// if the location wasn't already sane don't load it
return MAV_RESULT_DENIED ; // failed as the location is not valid
}
// we need to do this first, as we don't want to change the flight mode unless we can also set the target
if ( ! copter . mode_guided . set_destination ( request_location , false , 0 , false , 0 ) ) {
return MAV_RESULT_FAILED ;
}
if ( ! copter . flightmode - > in_guided_mode ( ) ) {
if ( ! copter . set_mode ( Mode : : Number : : GUIDED , ModeReason : : GCS_COMMAND ) ) {
return MAV_RESULT_FAILED ;
}
// the position won't have been loaded if we had to change the flight mode, so load it again
if ( ! copter . mode_guided . set_destination ( request_location , false , 0 , false , 0 ) ) {
return MAV_RESULT_FAILED ;
}
}
return MAV_RESULT_ACCEPTED ;
}
2018-07-03 22:43:51 -03:00
MAV_RESULT GCS_MAVLINK_Copter : : handle_command_int_packet ( const mavlink_command_int_t & packet )
{
switch ( packet . command ) {
case MAV_CMD_DO_FOLLOW :
# if MODE_FOLLOW_ENABLED == ENABLED
// param1: sysid of target to follow
if ( ( packet . param1 > 0 ) & & ( packet . param1 < = 255 ) ) {
copter . g2 . follow . set_target_sysid ( ( uint8_t ) packet . param1 ) ;
return MAV_RESULT_ACCEPTED ;
}
# endif
return MAV_RESULT_UNSUPPORTED ;
2020-06-30 17:48:11 -03:00
case MAV_CMD_DO_REPOSITION :
return handle_command_int_do_reposition ( packet ) ;
2018-07-03 22:43:51 -03:00
default :
return GCS_MAVLINK : : handle_command_int_packet ( packet ) ;
}
}
2018-10-13 19:24:43 -03:00
MAV_RESULT GCS_MAVLINK_Copter : : handle_command_mount ( const mavlink_command_long_t & packet )
{
// if the mount doesn't do pan control then yaw the entire vehicle instead:
switch ( packet . command ) {
2020-07-24 14:02:40 -03:00
# if HAL_MOUNT_ENABLED
2018-10-13 19:24:43 -03:00
case MAV_CMD_DO_MOUNT_CONTROL :
2020-02-17 01:42:20 -04:00
if ( ! copter . camera_mount . has_pan_control ( ) ) {
2018-10-13 19:24:43 -03:00
copter . flightmode - > auto_yaw . set_fixed_yaw (
2019-04-23 14:26:30 -03:00
( float ) packet . param3 * 0.01f ,
2018-10-13 19:24:43 -03:00
0.0f ,
0 , 0 ) ;
}
break ;
# endif
default :
break ;
}
return GCS_MAVLINK : : handle_command_mount ( packet ) ;
}
2018-07-03 23:16:58 -03:00
2019-05-05 22:39:57 -03:00
bool GCS_MAVLINK_Copter : : allow_disarm ( ) const
{
return copter . ap . land_complete ;
}
2018-07-03 23:16:58 -03:00
MAV_RESULT GCS_MAVLINK_Copter : : handle_command_long_packet ( const mavlink_command_long_t & packet )
2011-02-21 02:36:05 -04:00
{
2018-07-03 23:16:58 -03:00
switch ( packet . command ) {
case MAV_CMD_NAV_TAKEOFF : {
// param3 : horizontal navigation by pilot acceptable
// param4 : yaw angle (not supported)
// param5 : latitude (not supported)
// param6 : longitude (not supported)
// param7 : altitude [metres]
float takeoff_alt = packet . param7 * 100 ; // Convert m to cm
if ( ! copter . flightmode - > do_user_takeoff ( takeoff_alt , is_zero ( packet . param3 ) ) ) {
return MAV_RESULT_FAILED ;
}
return MAV_RESULT_ACCEPTED ;
}
2020-07-16 15:36:48 -03:00
case MAV_CMD_DO_LAND_START :
if ( copter . mode_auto . mission . jump_to_landing_sequence ( ) & & copter . set_mode ( Mode : : Number : : AUTO , ModeReason : : GCS_COMMAND ) ) {
return MAV_RESULT_ACCEPTED ;
}
return MAV_RESULT_FAILED ;
2018-07-03 23:16:58 -03:00
case MAV_CMD_NAV_LOITER_UNLIM :
2019-10-17 00:49:22 -03:00
if ( ! copter . set_mode ( Mode : : Number : : LOITER , ModeReason : : GCS_COMMAND ) ) {
2018-07-03 23:16:58 -03:00
return MAV_RESULT_FAILED ;
}
return MAV_RESULT_ACCEPTED ;
case MAV_CMD_NAV_RETURN_TO_LAUNCH :
2019-10-17 00:49:22 -03:00
if ( ! copter . set_mode ( Mode : : Number : : RTL , ModeReason : : GCS_COMMAND ) ) {
2018-07-03 23:16:58 -03:00
return MAV_RESULT_FAILED ;
}
return MAV_RESULT_ACCEPTED ;
case MAV_CMD_NAV_LAND :
2019-10-17 00:49:22 -03:00
if ( ! copter . set_mode ( Mode : : Number : : LAND , ModeReason : : GCS_COMMAND ) ) {
2018-07-03 23:16:58 -03:00
return MAV_RESULT_FAILED ;
}
return MAV_RESULT_ACCEPTED ;
# if MODE_FOLLOW_ENABLED == ENABLED
case MAV_CMD_DO_FOLLOW :
// param1: sysid of target to follow
if ( ( packet . param1 > 0 ) & & ( packet . param1 < = 255 ) ) {
copter . g2 . follow . set_target_sysid ( ( uint8_t ) packet . param1 ) ;
return MAV_RESULT_ACCEPTED ;
}
return MAV_RESULT_FAILED ;
# endif
case MAV_CMD_CONDITION_YAW :
// param1 : target angle [0-360]
// param2 : speed during change [deg per second]
// param3 : direction (-1:ccw, +1:cw)
// param4 : relative offset (1) or absolute angle (0)
if ( ( packet . param1 > = 0.0f ) & &
( packet . param1 < = 360.0f ) & &
( is_zero ( packet . param4 ) | | is_equal ( packet . param4 , 1.0f ) ) ) {
copter . flightmode - > auto_yaw . set_fixed_yaw (
packet . param1 ,
packet . param2 ,
( int8_t ) packet . param3 ,
is_positive ( packet . param4 ) ) ;
return MAV_RESULT_ACCEPTED ;
}
return MAV_RESULT_FAILED ;
case MAV_CMD_DO_CHANGE_SPEED :
// param1 : unused
// param2 : new speed in m/s
// param3 : unused
// param4 : unused
if ( packet . param2 > 0.0f ) {
2018-10-09 08:46:37 -03:00
if ( packet . param1 > 2.9f ) { // 3 = speed down
2019-01-24 01:02:31 -04:00
copter . wp_nav - > set_speed_down ( packet . param2 * 100.0f ) ;
2018-10-15 00:28:33 -03:00
} else if ( packet . param1 > 1.9f ) { // 2 = speed up
2019-01-24 01:02:31 -04:00
copter . wp_nav - > set_speed_up ( packet . param2 * 100.0f ) ;
2018-10-09 08:46:37 -03:00
} else {
copter . wp_nav - > set_speed_xy ( packet . param2 * 100.0f ) ;
}
2018-07-03 23:16:58 -03:00
return MAV_RESULT_ACCEPTED ;
}
return MAV_RESULT_FAILED ;
# if MODE_AUTO_ENABLED == ENABLED
case MAV_CMD_MISSION_START :
2019-09-07 20:33:56 -03:00
if ( copter . motors - > armed ( ) & &
2019-10-17 00:49:22 -03:00
copter . set_mode ( Mode : : Number : : AUTO , ModeReason : : GCS_COMMAND ) ) {
2018-07-03 23:16:58 -03:00
copter . set_auto_armed ( true ) ;
2018-04-24 20:31:01 -03:00
if ( copter . mode_auto . mission . state ( ) ! = AP_Mission : : MISSION_RUNNING ) {
copter . mode_auto . mission . start_or_resume ( ) ;
2018-07-03 23:16:58 -03:00
}
return MAV_RESULT_ACCEPTED ;
}
return MAV_RESULT_FAILED ;
# endif
# if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE :
// configure or release parachute
switch ( ( uint16_t ) packet . param1 ) {
case PARACHUTE_DISABLE :
copter . parachute . enabled ( false ) ;
2019-10-25 03:06:05 -03:00
AP : : logger ( ) . Write_Event ( LogEvent : : PARACHUTE_DISABLED ) ;
2018-07-03 23:16:58 -03:00
return MAV_RESULT_ACCEPTED ;
case PARACHUTE_ENABLE :
copter . parachute . enabled ( true ) ;
2019-10-25 03:06:05 -03:00
AP : : logger ( ) . Write_Event ( LogEvent : : PARACHUTE_ENABLED ) ;
2018-07-03 23:16:58 -03:00
return MAV_RESULT_ACCEPTED ;
case PARACHUTE_RELEASE :
// treat as a manual release which performs some additional check of altitude
copter . parachute_manual_release ( ) ;
return MAV_RESULT_ACCEPTED ;
}
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 : num_motors (in sequence)
// param6 : compass learning (0: disabled, 1: enabled)
2019-06-24 23:09:15 -03:00
return copter . mavlink_motor_test_start ( * this ,
2018-07-03 23:16:58 -03:00
( uint8_t ) packet . param1 ,
( uint8_t ) packet . param2 ,
( uint16_t ) packet . param3 ,
packet . param4 ,
( uint8_t ) packet . param5 ) ;
# if WINCH_ENABLED == ENABLED
case MAV_CMD_DO_WINCH :
// param1 : winch number (ignored)
// param2 : action (0=relax, 1=relative length control, 2=rate control). See WINCH_ACTIONS enum.
if ( ! copter . g2 . winch . enabled ( ) ) {
return MAV_RESULT_FAILED ;
}
switch ( ( uint8_t ) packet . param2 ) {
case WINCH_RELAXED :
copter . g2 . winch . relax ( ) ;
return MAV_RESULT_ACCEPTED ;
case WINCH_RELATIVE_LENGTH_CONTROL : {
2020-07-24 05:38:12 -03:00
copter . g2 . winch . release_length ( packet . param3 ) ;
2018-07-03 23:16:58 -03:00
return MAV_RESULT_ACCEPTED ;
}
case WINCH_RATE_CONTROL :
2020-07-24 05:38:12 -03:00
copter . g2 . winch . set_desired_rate ( packet . param4 ) ;
return MAV_RESULT_ACCEPTED ;
2018-07-03 23:16:58 -03:00
default :
break ;
}
return MAV_RESULT_FAILED ;
# endif
2018-07-30 19:13:37 -03:00
case MAV_CMD_AIRFRAME_CONFIGURATION : {
// Param 1: Select which gear, not used in ArduPilot
// Param 2: 0 = Deploy, 1 = Retract
// For safety, anything other than 1 will deploy
switch ( ( uint8_t ) packet . param2 ) {
case 1 :
copter . landinggear . set_position ( AP_LandingGear : : LandingGear_Retract ) ;
return MAV_RESULT_ACCEPTED ;
default :
copter . landinggear . set_position ( AP_LandingGear : : LandingGear_Deploy ) ;
return MAV_RESULT_ACCEPTED ;
}
return MAV_RESULT_FAILED ;
}
2018-07-03 23:16:58 -03:00
/* Solo user presses Fly button */
case MAV_CMD_SOLO_BTN_FLY_CLICK : {
if ( copter . failsafe . radio ) {
return MAV_RESULT_ACCEPTED ;
}
// set mode to Loiter or fall back to AltHold
2019-10-17 00:49:22 -03:00
if ( ! copter . set_mode ( Mode : : Number : : LOITER , ModeReason : : GCS_COMMAND ) ) {
copter . set_mode ( Mode : : Number : : ALT_HOLD , ModeReason : : GCS_COMMAND ) ;
2018-07-03 23:16:58 -03:00
}
return MAV_RESULT_ACCEPTED ;
}
/* Solo user holds down Fly button for a couple of seconds */
case MAV_CMD_SOLO_BTN_FLY_HOLD : {
if ( copter . failsafe . radio ) {
return MAV_RESULT_ACCEPTED ;
}
if ( ! copter . motors - > armed ( ) ) {
// if disarmed, arm motors
2019-05-05 22:39:57 -03:00
copter . arming . arm ( AP_Arming : : Method : : MAVLINK ) ;
2018-07-03 23:16:58 -03:00
} else if ( copter . ap . land_complete ) {
// if armed and landed, takeoff
2019-10-17 00:49:22 -03:00
if ( copter . set_mode ( Mode : : Number : : LOITER , ModeReason : : GCS_COMMAND ) ) {
2018-07-03 23:16:58 -03:00
copter . flightmode - > do_user_takeoff ( packet . param1 * 100 , true ) ;
}
} else {
// if flying, land
2019-10-17 00:49:22 -03:00
copter . set_mode ( Mode : : Number : : LAND , ModeReason : : GCS_COMMAND ) ;
2018-07-03 23:16:58 -03:00
}
return MAV_RESULT_ACCEPTED ;
}
/* Solo user presses pause button */
case MAV_CMD_SOLO_BTN_PAUSE_CLICK : {
if ( copter . failsafe . radio ) {
return MAV_RESULT_ACCEPTED ;
}
if ( copter . motors - > armed ( ) ) {
if ( copter . ap . land_complete ) {
// if landed, disarm motors
2020-02-21 09:09:57 -04:00
copter . arming . disarm ( AP_Arming : : Method : : SOLOPAUSEWHENLANDED ) ;
2018-07-03 23:16:58 -03:00
} else {
// assume that shots modes are all done in guided.
// NOTE: this may need to change if we add a non-guided shot mode
2019-09-07 20:33:56 -03:00
bool shot_mode = ( ! is_zero ( packet . param1 ) & & ( copter . control_mode = = Mode : : Number : : GUIDED | | copter . control_mode = = Mode : : Number : : GUIDED_NOGPS ) ) ;
2018-07-03 23:16:58 -03:00
if ( ! shot_mode ) {
# if MODE_BRAKE_ENABLED == ENABLED
2019-10-17 00:49:22 -03:00
if ( copter . set_mode ( Mode : : Number : : BRAKE , ModeReason : : GCS_COMMAND ) ) {
2018-07-03 23:16:58 -03:00
copter . mode_brake . timeout_to_loiter_ms ( 2500 ) ;
} else {
2019-10-17 00:49:22 -03:00
copter . set_mode ( Mode : : Number : : ALT_HOLD , ModeReason : : GCS_COMMAND ) ;
2018-07-03 23:16:58 -03:00
}
# else
2019-10-17 00:49:22 -03:00
copter . set_mode ( Mode : : Number : : ALT_HOLD , ModeReason : : GCS_COMMAND ) ;
2018-07-03 23:16:58 -03:00
# endif
} else {
// SoloLink is expected to handle pause in shots
}
}
}
return MAV_RESULT_ACCEPTED ;
}
default :
return GCS_MAVLINK : : handle_command_long_packet ( packet ) ;
}
}
2019-04-30 07:22:50 -03:00
void GCS_MAVLINK_Copter : : handle_mount_message ( const mavlink_message_t & msg )
2018-10-13 19:24:43 -03:00
{
2019-04-30 07:22:50 -03:00
switch ( msg . msgid ) {
2020-07-24 14:02:40 -03:00
# if HAL_MOUNT_ENABLED
2018-10-13 19:24:43 -03:00
case MAVLINK_MSG_ID_MOUNT_CONTROL :
2020-02-17 01:42:20 -04:00
if ( ! copter . camera_mount . has_pan_control ( ) ) {
2018-10-13 19:24:43 -03:00
// if the mount doesn't do pan control then yaw the entire vehicle instead:
copter . flightmode - > auto_yaw . set_fixed_yaw (
2019-04-30 07:22:50 -03:00
mavlink_msg_mount_control_get_input_c ( & msg ) * 0.01f ,
2018-10-13 19:24:43 -03:00
0.0f ,
0 ,
0 ) ;
break ;
}
# endif
}
GCS_MAVLINK : : handle_mount_message ( msg ) ;
}
2018-07-03 23:16:58 -03:00
2019-04-30 07:22:50 -03:00
void GCS_MAVLINK_Copter : : handleMessage ( const mavlink_message_t & msg )
2018-07-03 23:16:58 -03:00
{
2019-04-30 07:22:50 -03:00
switch ( msg . msgid ) {
2011-02-21 02:36:05 -04:00
2014-02-27 21:24:47 -04:00
case MAVLINK_MSG_ID_HEARTBEAT : // MAV ID: 0
2012-08-16 21:50:03 -03:00
{
2014-02-27 21:24:47 -04:00
// We keep track of the last time we received a heartbeat from our GCS for failsafe purposes
2020-02-17 01:42:20 -04:00
if ( msg . sysid ! = copter . g . sysid_my_gcs ) break ;
2015-11-19 23:04:45 -04:00
copter . failsafe . last_heartbeat_ms = AP_HAL : : millis ( ) ;
2014-02-27 21:24:47 -04:00
break ;
}
2012-04-24 06:53:24 -03:00
2017-03-18 22:04:36 -03:00
case MAVLINK_MSG_ID_MANUAL_CONTROL :
{
2019-04-30 07:22:50 -03:00
if ( msg . sysid ! = copter . g . sysid_my_gcs ) {
2018-04-03 20:20:31 -03:00
break ; // only accept control from our gcs
}
2017-03-18 22:04:36 -03:00
mavlink_manual_control_t packet ;
2019-04-30 07:22:50 -03:00
mavlink_msg_manual_control_decode ( & msg , & packet ) ;
2017-03-18 22:04:36 -03:00
2018-04-03 20:20:31 -03:00
if ( packet . target ! = copter . g . sysid_this_mav ) {
break ; // only accept control aimed at us
}
2017-03-18 22:04:36 -03:00
if ( packet . z < 0 ) { // Copter doesn't do negative thrust
break ;
}
2018-04-26 20:45:46 -03:00
uint32_t tnow = AP_HAL : : millis ( ) ;
2019-03-26 09:29:53 -03:00
manual_override ( copter . channel_roll , packet . y , 1000 , 2000 , tnow ) ;
manual_override ( copter . channel_pitch , packet . x , 1000 , 2000 , tnow , true ) ;
manual_override ( copter . channel_throttle , packet . z , 0 , 1000 , tnow ) ;
manual_override ( copter . channel_yaw , packet . r , 1000 , 2000 , tnow ) ;
2017-03-18 22:04:36 -03:00
// a manual control message is considered to be a 'heartbeat' from the ground station for failsafe purposes
2018-04-26 20:45:46 -03:00
copter . failsafe . last_heartbeat_ms = tnow ;
2017-03-18 22:04:36 -03:00
break ;
2012-08-16 21:50:03 -03:00
}
2012-06-26 02:19:51 -03:00
2018-02-23 00:09:07 -04:00
# if MODE_GUIDED_ENABLED == ENABLED
2015-10-08 08:13:53 -03:00
case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET : // MAV ID: 82
{
// decode packet
mavlink_set_attitude_target_t packet ;
2019-04-30 07:22:50 -03:00
mavlink_msg_set_attitude_target_decode ( & msg , & packet ) ;
2015-10-08 08:13:53 -03:00
2016-08-01 05:43:15 -03:00
// exit if vehicle is not in Guided mode or Auto-Guided mode
2018-02-21 21:49:47 -04:00
if ( ! copter . flightmode - > in_guided_mode ( ) ) {
2016-08-01 05:43:15 -03:00
break ;
}
2015-10-10 09:43:43 -03:00
// ensure type_mask specifies to use attitude and thrust
if ( ( packet . type_mask & ( ( 1 < < 7 ) | ( 1 < < 6 ) ) ) ! = 0 ) {
2015-10-08 08:13:53 -03:00
break ;
}
2020-07-27 06:18:34 -03:00
// check if the message's thrust field should be interpreted as a climb rate or as thrust
const bool use_thrust = copter . g2 . dev_options . get ( ) & DevOptionSetAttitudeTarget_ThrustAsThrust ;
float climb_rate_or_thrust ;
if ( use_thrust ) {
// interpret thrust as thrust
climb_rate_or_thrust = constrain_float ( packet . thrust , - 1.0f , 1.0f ) ;
2015-10-08 08:13:53 -03:00
} else {
2020-07-27 06:18:34 -03:00
// convert thrust to climb rate
packet . thrust = constrain_float ( packet . thrust , 0.0f , 1.0f ) ;
if ( is_equal ( packet . thrust , 0.5f ) ) {
climb_rate_or_thrust = 0.0f ;
} else if ( packet . thrust > 0.5f ) {
// climb at up to WPNAV_SPEED_UP
climb_rate_or_thrust = ( packet . thrust - 0.5f ) * 2.0f * copter . wp_nav - > get_default_speed_up ( ) ;
} else {
// descend at up to WPNAV_SPEED_DN
climb_rate_or_thrust = ( 0.5f - packet . thrust ) * 2.0f * - fabsf ( copter . wp_nav - > get_default_speed_down ( ) ) ;
}
2015-10-08 08:13:53 -03:00
}
2016-08-02 14:20:57 -03:00
// if the body_yaw_rate field is ignored, use the commanded yaw position
// otherwise use the commanded yaw rate
bool use_yaw_rate = false ;
if ( ( packet . type_mask & ( 1 < < 2 ) ) = = 0 ) {
use_yaw_rate = true ;
}
2017-12-11 01:43:27 -04:00
copter . mode_guided . set_angle ( Quaternion ( packet . q [ 0 ] , packet . q [ 1 ] , packet . q [ 2 ] , packet . q [ 3 ] ) ,
2020-07-27 06:18:34 -03:00
climb_rate_or_thrust , use_yaw_rate , packet . body_yaw_rate , use_thrust ) ;
2016-08-02 14:20:57 -03:00
2015-10-08 08:13:53 -03:00
break ;
}
2014-10-13 08:31:40 -03:00
case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED : // MAV ID: 84
{
// decode packet
mavlink_set_position_target_local_ned_t packet ;
2019-04-30 07:22:50 -03:00
mavlink_msg_set_position_target_local_ned_decode ( & msg , & packet ) ;
2014-10-13 08:31:40 -03:00
2014-10-13 23:26:08 -03:00
// exit if vehicle is not in Guided mode or Auto-Guided mode
2018-02-21 21:49:47 -04:00
if ( ! copter . flightmode - > in_guided_mode ( ) ) {
2014-10-13 08:31:40 -03:00
break ;
}
2015-08-11 08:26:28 -03:00
// check for supported coordinate frames
if ( packet . coordinate_frame ! = MAV_FRAME_LOCAL_NED & &
packet . coordinate_frame ! = MAV_FRAME_LOCAL_OFFSET_NED & &
packet . coordinate_frame ! = MAV_FRAME_BODY_NED & &
packet . coordinate_frame ! = MAV_FRAME_BODY_OFFSET_NED ) {
break ;
}
2014-11-14 21:18:17 -04:00
bool pos_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ;
bool vel_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE ;
bool acc_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE ;
2017-06-22 12:20:14 -03:00
bool yaw_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE ;
bool yaw_rate_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE ;
2014-11-14 21:18:17 -04:00
2020-02-27 07:34:32 -04:00
// exit immediately if acceleration provided
if ( ! acc_ignore ) {
break ;
}
2014-11-14 21:18:17 -04:00
2015-08-11 08:26:28 -03:00
// prepare position
Vector3f pos_vector ;
if ( ! pos_ignore ) {
// convert to cm
pos_vector = Vector3f ( packet . x * 100.0f , packet . y * 100.0f , - packet . z * 100.0f ) ;
// rotate to body-frame if necessary
if ( packet . coordinate_frame = = MAV_FRAME_BODY_NED | |
packet . coordinate_frame = = MAV_FRAME_BODY_OFFSET_NED ) {
copter . rotate_body_frame_to_NE ( pos_vector . x , pos_vector . y ) ;
}
// add body offset if necessary
if ( packet . coordinate_frame = = MAV_FRAME_LOCAL_OFFSET_NED | |
packet . coordinate_frame = = MAV_FRAME_BODY_NED | |
packet . coordinate_frame = = MAV_FRAME_BODY_OFFSET_NED ) {
pos_vector + = copter . inertial_nav . get_position ( ) ;
} else {
// convert from alt-above-home to alt-above-ekf-origin
2019-02-21 02:42:13 -04:00
if ( ! AP : : ahrs ( ) . home_is_set ( ) ) {
break ;
}
2019-06-28 10:55:36 -03:00
Location origin ;
2019-02-21 02:42:13 -04:00
pos_vector . z + = AP : : ahrs ( ) . get_home ( ) . alt ;
2019-06-28 10:55:36 -03:00
if ( copter . ahrs . get_origin ( origin ) ) {
pos_vector . z - = origin . alt ;
}
2015-08-11 08:26:28 -03:00
}
}
// prepare velocity
Vector3f vel_vector ;
if ( ! vel_ignore ) {
// convert to cm
vel_vector = Vector3f ( packet . vx * 100.0f , packet . vy * 100.0f , - packet . vz * 100.0f ) ;
// rotate to body-frame if necessary
if ( packet . coordinate_frame = = MAV_FRAME_BODY_NED | | packet . coordinate_frame = = MAV_FRAME_BODY_OFFSET_NED ) {
copter . rotate_body_frame_to_NE ( vel_vector . x , vel_vector . y ) ;
}
}
2017-07-11 02:06:18 -03:00
// prepare yaw
float yaw_cd = 0.0f ;
bool yaw_relative = false ;
float yaw_rate_cds = 0.0f ;
if ( ! yaw_ignore ) {
yaw_cd = ToDeg ( packet . yaw ) * 100.0f ;
yaw_relative = packet . coordinate_frame = = MAV_FRAME_BODY_NED | | packet . coordinate_frame = = MAV_FRAME_BODY_OFFSET_NED ;
}
if ( ! yaw_rate_ignore ) {
yaw_rate_cds = ToDeg ( packet . yaw_rate ) * 100.0f ;
}
2015-08-11 08:26:28 -03:00
// send request
2020-02-27 07:34:32 -04:00
if ( ! pos_ignore & & ! vel_ignore ) {
2018-07-03 23:16:58 -03:00
copter . mode_guided . set_destination_posvel ( pos_vector , vel_vector , ! yaw_ignore , yaw_cd , ! yaw_rate_ignore , yaw_rate_cds , yaw_relative ) ;
2020-02-27 07:34:32 -04:00
} else if ( pos_ignore & & ! vel_ignore ) {
2017-12-11 01:43:27 -04:00
copter . mode_guided . set_velocity ( vel_vector , ! yaw_ignore , yaw_cd , ! yaw_rate_ignore , yaw_rate_cds , yaw_relative ) ;
2020-02-27 07:34:32 -04:00
} else if ( ! pos_ignore & & vel_ignore ) {
2018-07-03 23:16:58 -03:00
copter . mode_guided . set_destination ( pos_vector , ! yaw_ignore , yaw_cd , ! yaw_rate_ignore , yaw_rate_cds , yaw_relative ) ;
2014-11-14 21:18:17 -04:00
}
2014-10-13 08:31:40 -03:00
break ;
}
case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT : // MAV ID: 86
{
// decode packet
mavlink_set_position_target_global_int_t packet ;
2019-04-30 07:22:50 -03:00
mavlink_msg_set_position_target_global_int_decode ( & msg , & packet ) ;
2014-10-13 08:31:40 -03:00
2014-10-13 23:26:08 -03:00
// exit if vehicle is not in Guided mode or Auto-Guided mode
2018-02-21 21:49:47 -04:00
if ( ! copter . flightmode - > in_guided_mode ( ) ) {
2014-10-13 08:31:40 -03:00
break ;
}
2014-11-14 21:18:17 -04:00
bool pos_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE ;
bool vel_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE ;
bool acc_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE ;
2017-06-22 12:20:14 -03:00
bool yaw_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE ;
bool yaw_rate_ignore = packet . type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE ;
2014-11-14 21:18:17 -04:00
2020-02-27 07:34:32 -04:00
// exit immediately if acceleration provided
if ( ! acc_ignore ) {
break ;
}
2014-11-14 21:18:17 -04:00
2020-02-27 07:23:45 -04:00
// extract location from message
Location loc ;
2020-02-17 01:42:20 -04:00
if ( ! pos_ignore ) {
2016-06-01 19:29:40 -03:00
// sanity check location
if ( ! check_latlng ( packet . lat_int , packet . lon_int ) ) {
break ;
}
2019-03-14 22:44:27 -03:00
Location : : AltFrame frame ;
2019-07-31 22:02:41 -03:00
if ( ! mavlink_coordinate_frame_to_location_alt_frame ( ( MAV_FRAME ) packet . coordinate_frame , frame ) ) {
2019-02-05 17:46:57 -04:00
// unknown coordinate frame
break ;
2014-11-14 21:18:17 -04:00
}
2020-02-27 07:23:45 -04:00
loc = { packet . lat_int , packet . lon_int , int32_t ( packet . alt * 100 ) , frame } ;
2014-11-14 21:18:17 -04:00
}
2017-07-11 02:06:18 -03:00
// prepare yaw
float yaw_cd = 0.0f ;
bool yaw_relative = false ;
float yaw_rate_cds = 0.0f ;
if ( ! yaw_ignore ) {
yaw_cd = ToDeg ( packet . yaw ) * 100.0f ;
yaw_relative = packet . coordinate_frame = = MAV_FRAME_BODY_NED | | packet . coordinate_frame = = MAV_FRAME_BODY_OFFSET_NED ;
}
if ( ! yaw_rate_ignore ) {
yaw_rate_cds = ToDeg ( packet . yaw_rate ) * 100.0f ;
}
2020-02-27 07:23:45 -04:00
// send targets to the appropriate guided mode controller
2020-02-27 07:34:32 -04:00
if ( ! pos_ignore & & ! vel_ignore ) {
2020-02-27 07:23:45 -04:00
// convert Location to vector from ekf origin for posvel controller
if ( loc . get_alt_frame ( ) = = Location : : AltFrame : : ABOVE_TERRAIN ) {
// posvel controller does not support alt-above-terrain
break ;
}
Vector3f pos_neu_cm ;
if ( ! loc . get_vector_from_origin_NEU ( pos_neu_cm ) ) {
break ;
}
2018-07-03 23:16:58 -03:00
copter . mode_guided . set_destination_posvel ( pos_neu_cm , Vector3f ( packet . vx * 100.0f , packet . vy * 100.0f , - packet . vz * 100.0f ) , ! yaw_ignore , yaw_cd , ! yaw_rate_ignore , yaw_rate_cds , yaw_relative ) ;
2020-02-27 07:34:32 -04:00
} else if ( pos_ignore & & ! vel_ignore ) {
2017-12-11 01:43:27 -04:00
copter . mode_guided . set_velocity ( Vector3f ( packet . vx * 100.0f , packet . vy * 100.0f , - packet . vz * 100.0f ) , ! yaw_ignore , yaw_cd , ! yaw_rate_ignore , yaw_rate_cds , yaw_relative ) ;
2020-02-27 07:34:32 -04:00
} else if ( ! pos_ignore & & vel_ignore ) {
2020-02-27 07:23:45 -04:00
copter . mode_guided . set_destination ( loc , ! yaw_ignore , yaw_cd , ! yaw_rate_ignore , yaw_rate_cds , yaw_relative ) ;
2014-11-14 21:18:17 -04:00
}
2014-10-13 08:31:40 -03:00
break ;
}
2018-02-23 00:09:07 -04:00
# endif
2014-10-13 08:31:40 -03:00
2012-07-04 21:55:58 -03:00
# if HIL_MODE != HIL_MODE_DISABLED
2014-02-27 21:24:47 -04:00
case MAVLINK_MSG_ID_HIL_STATE : // MAV ID: 90
2012-08-16 21:50:03 -03:00
{
mavlink_hil_state_t packet ;
2019-04-30 07:22:50 -03:00
mavlink_msg_hil_state_decode ( & msg , & packet ) ;
2012-06-26 02:19:51 -03:00
2016-06-01 19:29:40 -03:00
// sanity check location
if ( ! check_latlng ( packet . lat , packet . lon ) ) {
break ;
}
2012-08-16 21:50:03 -03:00
// set gps hil sensor
2014-03-31 04:07:46 -03:00
Location loc ;
loc . lat = packet . lat ;
loc . lng = packet . lon ;
loc . alt = packet . alt / 10 ;
Vector3f vel ( packet . vx , packet . vy , packet . vz ) ;
vel * = 0.01f ;
2014-04-01 17:51:19 -03:00
gps . setHIL ( 0 , AP_GPS : : GPS_OK_FIX_3D ,
2014-03-31 04:07:46 -03:00
packet . time_usec / 1000 ,
2016-05-04 22:42:28 -03:00
loc , vel , 10 , 0 ) ;
2012-06-26 02:19:51 -03:00
2012-08-16 21:50:03 -03:00
// rad/sec
Vector3f gyros ;
gyros . x = packet . rollspeed ;
gyros . y = packet . pitchspeed ;
gyros . z = packet . yawspeed ;
2013-03-19 03:07:15 -03:00
2012-08-16 21:50:03 -03:00
// m/s/s
Vector3f accels ;
2015-05-02 06:26:19 -03:00
accels . x = packet . xacc * ( GRAVITY_MSS / 1000.0f ) ;
accels . y = packet . yacc * ( GRAVITY_MSS / 1000.0f ) ;
accels . z = packet . zacc * ( GRAVITY_MSS / 1000.0f ) ;
2012-06-04 05:24:08 -03:00
2014-02-22 17:18:20 -04:00
ins . set_gyro ( 0 , gyros ) ;
2012-06-04 05:24:08 -03:00
2014-02-22 17:18:20 -04:00
ins . set_accel ( 0 , accels ) ;
2012-06-26 02:19:51 -03:00
2018-03-05 16:37:25 -04:00
AP : : baro ( ) . setHIL ( packet . alt * 0.001f ) ;
2015-05-29 23:12:49 -03:00
copter . compass . setHIL ( 0 , packet . roll , packet . pitch , packet . yaw ) ;
copter . compass . setHIL ( 1 , packet . roll , packet . pitch , packet . yaw ) ;
2012-06-04 05:24:08 -03:00
2012-08-16 21:50:03 -03:00
break ;
}
2012-07-04 21:55:58 -03:00
# endif // HIL_MODE != HIL_MODE_DISABLED
2012-06-26 02:19:51 -03:00
2014-02-27 21:24:47 -04:00
case MAVLINK_MSG_ID_RADIO :
case MAVLINK_MSG_ID_RADIO_STATUS : // MAV ID: 109
2013-04-29 09:30:22 -03:00
{
2019-03-26 02:13:36 -03:00
handle_radio_status ( msg , copter . should_log ( MASK_LOG_PM ) ) ;
2013-04-29 09:30:22 -03:00
break ;
}
2012-04-01 22:18:42 -03:00
2015-09-10 16:06:38 -03:00
# if PRECISION_LANDING == ENABLED
2016-03-20 14:12:31 -03:00
case MAVLINK_MSG_ID_LANDING_TARGET :
copter . precland . handle_msg ( msg ) ;
break ;
2015-09-10 16:06:38 -03:00
# endif
2014-07-22 05:22:36 -03:00
case MAVLINK_MSG_ID_TERRAIN_DATA :
case MAVLINK_MSG_ID_TERRAIN_CHECK :
2015-11-13 23:38:47 -04:00
# if AP_TERRAIN_AVAILABLE && AC_TERRAIN
2015-05-29 23:12:49 -03:00
copter . terrain . handle_data ( chan , msg ) ;
2014-07-22 05:22:36 -03:00
# endif
break ;
2015-10-02 22:37:02 -03:00
case MAVLINK_MSG_ID_SET_HOME_POSITION :
{
mavlink_set_home_position_t packet ;
2019-04-30 07:22:50 -03:00
mavlink_msg_set_home_position_decode ( & msg , & packet ) ;
2020-02-17 01:42:20 -04:00
if ( ( packet . latitude = = 0 ) & & ( packet . longitude = = 0 ) & & ( packet . altitude = = 0 ) ) {
2018-05-29 21:47:08 -03:00
if ( ! copter . set_home_to_current_location ( true ) ) {
// silently ignored
}
2015-10-02 22:37:02 -03:00
} else {
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 21:47:08 -03:00
if ( ! copter . set_home ( new_home_loc , true ) ) {
// silently ignored
}
2015-10-02 22:37:02 -03:00
}
break ;
}
2015-11-25 19:22:21 -04:00
case MAVLINK_MSG_ID_ADSB_VEHICLE :
2016-08-16 20:19:48 -03:00
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG :
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC :
2016-08-16 19:37:29 -03:00
case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT :
2016-07-10 21:21:57 -03:00
# if ADSB_ENABLED == ENABLED
2016-08-16 20:19:48 -03:00
copter . adsb . handle_message ( chan , msg ) ;
2016-07-10 21:21:57 -03:00
# endif
break ;
2018-01-18 02:49:20 -04:00
# if TOY_MODE_ENABLED == ENABLED
case MAVLINK_MSG_ID_NAMED_VALUE_INT :
copter . g2 . toy_mode . handle_message ( msg ) ;
break ;
# endif
2016-11-06 20:06:51 -04:00
default :
handle_common_message ( msg ) ;
2016-01-20 22:50:49 -04:00
break ;
2014-02-27 21:24:47 -04:00
} // end switch
} // end handle mavlink
2012-07-24 23:02:54 -03:00
2013-03-20 23:54:04 -03:00
2017-07-25 03:36:17 -03:00
MAV_RESULT GCS_MAVLINK_Copter : : handle_flight_termination ( const mavlink_command_long_t & packet ) {
MAV_RESULT result = MAV_RESULT_FAILED ;
# if ADVANCED_FAILSAFE == ENABLED
if ( GCS_MAVLINK : : handle_flight_termination ( packet ) ! = MAV_RESULT_ACCEPTED ) {
# endif
if ( packet . param1 > 0.5f ) {
2020-02-21 09:09:57 -04:00
copter . arming . disarm ( AP_Arming : : Method : : TERMINATION ) ;
2017-07-25 03:36:17 -03:00
result = MAV_RESULT_ACCEPTED ;
}
# if ADVANCED_FAILSAFE == ENABLED
} else {
result = MAV_RESULT_ACCEPTED ;
}
# endif
return result ;
}
2018-11-05 21:04:00 -04:00
float GCS_MAVLINK_Copter : : vfr_hud_alt ( ) const
{
if ( copter . g2 . dev_options . get ( ) & DevOptionVFR_HUDRelativeAlt ) {
2019-03-25 20:57:55 -03:00
// compatibility option for older mavlink-aware devices that
2018-11-05 21:04:00 -04:00
// assume Copter returns a relative altitude in VFR_HUD.alt
2019-04-23 14:26:30 -03:00
return copter . current_loc . alt * 0.01f ;
2018-11-05 21:04:00 -04:00
}
return GCS_MAVLINK : : vfr_hud_alt ( ) ;
}
2018-03-28 21:51:26 -03:00
uint64_t GCS_MAVLINK_Copter : : capabilities ( ) const
{
return ( MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT |
MAV_PROTOCOL_CAPABILITY_MISSION_INT |
MAV_PROTOCOL_CAPABILITY_COMMAND_INT |
MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED |
MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT |
MAV_PROTOCOL_CAPABILITY_FLIGHT_TERMINATION |
MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
# if AP_TERRAIN_AVAILABLE && AC_TERRAIN
( copter . terrain . enabled ( ) ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0 ) |
# endif
GCS_MAVLINK : : capabilities ( ) ) ;
}
2018-04-30 06:50:04 -03:00
MAV_LANDED_STATE GCS_MAVLINK_Copter : : landed_state ( ) const
{
if ( copter . ap . land_complete ) {
return MAV_LANDED_STATE_ON_GROUND ;
}
if ( copter . flightmode - > is_landing ( ) ) {
return MAV_LANDED_STATE_LANDING ;
}
if ( copter . flightmode - > is_taking_off ( ) ) {
return MAV_LANDED_STATE_TAKEOFF ;
}
return MAV_LANDED_STATE_IN_AIR ;
}