27 return copter.get_frame_mav_type();
32 uint8_t _base_mode = MAV_MODE_FLAG_STABILIZE_ENABLED;
41 switch (
copter.control_mode) {
52 _base_mode |= MAV_MODE_FLAG_GUIDED_ENABLED;
63 _base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
65 #if HIL_MODE != HIL_MODE_DISABLED 66 _base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
70 if (
copter.motors !=
nullptr &&
copter.motors->armed()) {
71 _base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
75 _base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
77 return (MAV_MODE)_base_mode;
82 return copter.control_mode;
89 if (
copter.any_failsafe_triggered()) {
90 return MAV_STATE_CRITICAL;
93 if (
copter.ap.land_complete) {
94 return MAV_STATE_STANDBY;
97 return MAV_STATE_ACTIVE;
104 if (!
copter.flightmode->get_wp(target)) {
107 mavlink_msg_position_target_global_int_send(
110 MAV_FRAME_GLOBAL_INT,
125 #if AC_FENCE == ENABLED 135 int16_t battery_current = -1;
136 int8_t battery_remaining = -1;
145 mavlink_msg_sys_status_send(
162 mavlink_msg_nav_controller_output_send(
176 return (int16_t)(
copter.motors->get_throttle() * 100);
184 #if RPM_ENABLED == ENABLED 186 mavlink_msg_rpm_send(
203 mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL,
216 mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
229 mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
242 mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ,
257 return copter.g.sysid_my_gcs;
262 return (uint32_t)(
copter.g.telem_delay);
268 if (telemetry_delayed()) {
272 #if HIL_MODE != HIL_MODE_SENSORS 280 if (
copter.motors !=
nullptr &&
copter.scheduler.time_available_usec() < 250 &&
copter.motors->armed()) {
291 if (
copter.ap.initialised) {
305 #if RPM_ENABLED == ENABLED 312 #if AP_TERRAIN_AVAILABLE && AC_TERRAIN 319 #if AC_FENCE == ENABLED 329 #endif // MOUNT == ENABLED 333 #if OPTFLOW == ENABLED 335 send_opticalflow(
copter.optflow);
364 #if ADSB_ENABLED == ENABLED 510 #if AP_TERRAIN_AVAILABLE && AC_TERRAIN 543 #if MODE_AUTO_ENABLED == ENABLED 544 return copter.mode_auto.do_guided(cmd);
561 mavlink_message_t &msg)
563 #if ADSB_ENABLED == ENABLED 566 copter.avoidance_adsb.handle_msg(msg);
569 #if MODE_FOLLOW_ENABLED == ENABLED 571 copter.g2.follow.handle_msg(msg);
585 return copter.ap.initialised_params;
591 send_text(MAV_SEVERITY_INFO,
"Frame: %s",
copter.get_frame_string());
597 copter.command_ack_counter++;
613 MAV_RESULT
result = MAV_RESULT_FAILED;
615 switch (msg->msgid) {
617 case MAVLINK_MSG_ID_HEARTBEAT:
620 if(msg->sysid !=
copter.g.sysid_my_gcs)
break;
625 case MAVLINK_MSG_ID_PARAM_VALUE:
628 copter.camera_mount.handle_param_value(msg);
633 case MAVLINK_MSG_ID_GIMBAL_REPORT:
636 handle_gimbal_report(
copter.camera_mount, msg);
641 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
646 if(msg->sysid !=
copter.g.sysid_my_gcs)
break;
647 if (!
copter.ap.rc_override_enable) {
648 if (
copter.failsafe.rc_override_active) {
649 copter.failsafe.rc_override_active =
false;
654 mavlink_rc_channels_override_t packet;
655 bool override_active =
false;
656 mavlink_msg_rc_channels_override_decode(msg, &packet);
668 copter.failsafe.rc_override_active = override_active;
675 case MAVLINK_MSG_ID_MANUAL_CONTROL:
677 if (msg->sysid !=
copter.g.sysid_my_gcs) {
681 mavlink_manual_control_t packet;
682 mavlink_msg_manual_control_decode(msg, &packet);
684 if (packet.target !=
copter.g.sysid_this_mav) {
692 bool override_active =
false;
693 int16_t
roll = (packet.y == INT16_MAX) ? 0 :
copter.channel_roll->get_radio_min() + (
copter.channel_roll->get_radio_max() -
copter.channel_roll->get_radio_min()) * (packet.y + 1000) / 2000.0f;
694 int16_t
pitch = (packet.x == INT16_MAX) ? 0 :
copter.channel_pitch->get_radio_min() + (
copter.channel_pitch->get_radio_max() -
copter.channel_pitch->get_radio_min()) * (-packet.x + 1000) / 2000.0f;
695 int16_t throttle = (packet.z == INT16_MAX) ? 0 :
copter.channel_throttle->get_radio_min() + (
copter.channel_throttle->get_radio_max() -
copter.channel_throttle->get_radio_min()) * (packet.z) / 1000.0f;
696 int16_t yaw = (packet.r == INT16_MAX) ? 0 :
copter.channel_yaw->get_radio_min() + (
copter.channel_yaw->get_radio_max() -
copter.channel_yaw->get_radio_min()) * (packet.r + 1000) / 2000.0f;
704 copter.failsafe.rc_override_active = override_active;
711 case MAVLINK_MSG_ID_COMMAND_INT:
714 mavlink_command_int_t packet;
715 mavlink_msg_command_int_decode(msg, &packet);
716 switch(packet.command)
718 case MAV_CMD_DO_FOLLOW:
719 #if MODE_FOLLOW_ENABLED == ENABLED 721 if ((packet.param1 > 0) && (packet.param1 <= 255)) {
722 copter.g2.follow.set_target_sysid((uint8_t)packet.param1);
723 result = MAV_RESULT_ACCEPTED;
728 case MAV_CMD_DO_SET_HOME: {
730 result = MAV_RESULT_FAILED;
731 if (
is_equal(packet.param1, 1.0f)) {
733 if (
copter.set_home_to_current_location(
true)) {
734 result = MAV_RESULT_ACCEPTED;
743 if (packet.frame != MAV_FRAME_GLOBAL &&
744 packet.frame != MAV_FRAME_GLOBAL_INT &&
745 packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
746 packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
754 new_home_loc.
lat = packet.x;
755 new_home_loc.lng = packet.y;
756 new_home_loc.alt = packet.z * 100;
758 if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) {
763 new_home_loc.alt +=
copter.ahrs.get_home().alt;
765 if (
copter.set_home(new_home_loc,
true)) {
766 result = MAV_RESULT_ACCEPTED;
771 case MAV_CMD_DO_SET_ROI: {
784 roi_loc.
lat = packet.x;
785 roi_loc.
lng = packet.y;
786 roi_loc.
alt = (int32_t)(packet.z * 100.0f);
787 copter.flightmode->auto_yaw.set_roi(roi_loc);
788 result = MAV_RESULT_ACCEPTED;
793 result = MAV_RESULT_UNSUPPORTED;
798 mavlink_msg_command_ack_send_buf(msg,
chan, packet.command, result);
803 case MAVLINK_MSG_ID_COMMAND_LONG:
806 mavlink_command_long_t packet;
807 mavlink_msg_command_long_decode(msg, &packet);
809 switch(packet.command) {
811 case MAV_CMD_NAV_TAKEOFF: {
818 float takeoff_alt = packet.param7 * 100;
820 if(
copter.do_user_takeoff(takeoff_alt,
is_zero(packet.param3))) {
821 result = MAV_RESULT_ACCEPTED;
823 result = MAV_RESULT_FAILED;
829 case MAV_CMD_NAV_LOITER_UNLIM:
831 result = MAV_RESULT_ACCEPTED;
835 case MAV_CMD_NAV_RETURN_TO_LAUNCH:
837 result = MAV_RESULT_ACCEPTED;
841 case MAV_CMD_NAV_LAND:
843 result = MAV_RESULT_ACCEPTED;
847 case MAV_CMD_DO_FOLLOW:
848 #if MODE_FOLLOW_ENABLED == ENABLED 850 if ((packet.param1 > 0) && (packet.param1 <= 255)) {
851 copter.g2.follow.set_target_sysid((uint8_t)packet.param1);
852 result = MAV_RESULT_ACCEPTED;
857 case MAV_CMD_CONDITION_YAW:
862 if ((packet.param1 >= 0.0f) &&
863 (packet.param1 <= 360.0f) &&
865 copter.flightmode->auto_yaw.set_fixed_yaw(
868 (int8_t)packet.param3,
870 result = MAV_RESULT_ACCEPTED;
872 result = MAV_RESULT_FAILED;
876 case MAV_CMD_DO_CHANGE_SPEED:
881 if (packet.param2 > 0.0f) {
882 copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);
883 result = MAV_RESULT_ACCEPTED;
885 result = MAV_RESULT_FAILED;
889 case MAV_CMD_DO_SET_HOME:
894 result = MAV_RESULT_FAILED;
896 if (
copter.set_home_to_current_location(
true)) {
897 result = MAV_RESULT_ACCEPTED;
909 new_home_loc.
lat = (int32_t)(packet.param5 * 1.0e7f);
910 new_home_loc.
lng = (int32_t)(packet.param6 * 1.0e7f);
911 new_home_loc.
alt = (int32_t)(packet.param7 * 100.0f);
912 if (
copter.set_home(new_home_loc,
true)) {
913 result = MAV_RESULT_ACCEPTED;
918 case MAV_CMD_DO_SET_ROI:
930 roi_loc.
lat = (int32_t)(packet.param5 * 1.0e7f);
931 roi_loc.
lng = (int32_t)(packet.param6 * 1.0e7f);
932 roi_loc.
alt = (int32_t)(packet.param7 * 100.0f);
933 copter.flightmode->auto_yaw.set_roi(roi_loc);
934 result = MAV_RESULT_ACCEPTED;
937 case MAV_CMD_DO_MOUNT_CONTROL:
939 if(!
copter.camera_mount.has_pan_control()) {
940 copter.flightmode->auto_yaw.set_fixed_yaw(
941 (
float)packet.param3 / 100.0f,
945 copter.camera_mount.control(packet.param1, packet.param2, packet.param3, (MAV_MOUNT_MODE) packet.param7);
946 result = MAV_RESULT_ACCEPTED;
950 #if MODE_AUTO_ENABLED == ENABLED 951 case MAV_CMD_MISSION_START:
953 copter.set_auto_armed(
true);
955 copter.mission.start_or_resume();
957 result = MAV_RESULT_ACCEPTED;
962 case MAV_CMD_COMPONENT_ARM_DISARM:
965 if (
copter.init_arm_motors(
true)) {
966 result = MAV_RESULT_ACCEPTED;
968 }
else if (
is_zero(packet.param1) && (
copter.ap.land_complete ||
is_equal(packet.param2,21196.0f))) {
970 copter.init_disarm_motors();
971 result = MAV_RESULT_ACCEPTED;
973 result = MAV_RESULT_UNSUPPORTED;
977 case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
984 result = MAV_RESULT_ACCEPTED;
988 case MAV_CMD_DO_FENCE_ENABLE:
989 #if AC_FENCE == ENABLED 990 result = MAV_RESULT_ACCEPTED;
991 switch ((uint16_t)packet.param1) {
993 copter.fence.enable(
false);
996 copter.fence.enable(
true);
999 result = MAV_RESULT_FAILED;
1004 result = MAV_RESULT_FAILED;
1008 #if PARACHUTE == ENABLED 1009 case MAV_CMD_DO_PARACHUTE:
1011 result = MAV_RESULT_ACCEPTED;
1012 switch ((uint16_t)packet.param1) {
1013 case PARACHUTE_DISABLE:
1014 copter.parachute.enabled(
false);
1017 case PARACHUTE_ENABLE:
1018 copter.parachute.enabled(
true);
1021 case PARACHUTE_RELEASE:
1023 copter.parachute_manual_release();
1026 result = MAV_RESULT_FAILED;
1032 case MAV_CMD_DO_MOTOR_TEST:
1039 result =
copter.mavlink_motor_test_start(
chan, (uint8_t)packet.param1, (uint8_t)packet.param2, (uint16_t)packet.param3,
1040 packet.param4, (uint8_t)packet.param5);
1043 #if GRIPPER_ENABLED == ENABLED 1044 case MAV_CMD_DO_GRIPPER:
1047 if(!
copter.g2.gripper.enabled()) {
1048 result = MAV_RESULT_FAILED;
1050 result = MAV_RESULT_ACCEPTED;
1051 switch ((uint8_t)packet.param2) {
1052 case GRIPPER_ACTION_RELEASE:
1053 copter.g2.gripper.release();
1055 case GRIPPER_ACTION_GRAB:
1056 copter.g2.gripper.grab();
1059 result = MAV_RESULT_FAILED;
1066 #if WINCH_ENABLED == ENABLED 1067 case MAV_CMD_DO_WINCH:
1070 if (!
copter.g2.winch.enabled()) {
1071 result = MAV_RESULT_FAILED;
1073 result = MAV_RESULT_ACCEPTED;
1074 switch ((uint8_t)packet.param2) {
1079 case WINCH_RELATIVE_LENGTH_CONTROL: {
1080 copter.g2.winch.release_length(packet.param3, fabsf(packet.param4));
1084 case WINCH_RATE_CONTROL: {
1085 if (fabsf(packet.param4) <=
copter.g2.winch.get_rate_max()) {
1086 copter.g2.winch.set_desired_rate(packet.param4);
1089 result = MAV_RESULT_FAILED;
1094 result = MAV_RESULT_FAILED;
1102 case MAV_CMD_SOLO_BTN_FLY_CLICK: {
1103 result = MAV_RESULT_ACCEPTED;
1105 if (
copter.failsafe.radio) {
1117 case MAV_CMD_SOLO_BTN_FLY_HOLD: {
1118 result = MAV_RESULT_ACCEPTED;
1120 if (
copter.failsafe.radio) {
1124 if (!
copter.motors->armed()) {
1126 copter.init_arm_motors(
true);
1127 }
else if (
copter.ap.land_complete) {
1130 copter.do_user_takeoff(packet.param1*100,
true);
1140 case MAV_CMD_SOLO_BTN_PAUSE_CLICK: {
1141 result = MAV_RESULT_ACCEPTED;
1143 if (
copter.failsafe.radio) {
1147 if (
copter.motors->armed()) {
1148 if (
copter.ap.land_complete) {
1150 copter.init_disarm_motors();
1157 #if MODE_BRAKE_ENABLED == ENABLED 1159 copter.mode_brake.timeout_to_loiter_ms(2500);
1174 case MAV_CMD_ACCELCAL_VEHICLE_POS:
1175 result = MAV_RESULT_FAILED;
1177 if (
copter.ins.get_acal()->gcs_vehicle_position(packet.param1)) {
1178 result = MAV_RESULT_ACCEPTED;
1183 result = handle_command_long_message(packet);
1188 mavlink_msg_command_ack_send_buf(msg,
chan, packet.command, result);
1193 #if MODE_GUIDED_ENABLED == ENABLED 1194 case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
1197 mavlink_set_attitude_target_t packet;
1198 mavlink_msg_set_attitude_target_decode(msg, &packet);
1201 if (!
copter.flightmode->in_guided_mode()) {
1206 if ((packet.type_mask & ((1<<7)|(1<<6))) != 0) {
1213 if (
is_equal(packet.thrust, 0.5f)) {
1214 climb_rate_cms = 0.0f;
1215 }
else if (packet.thrust > 0.5f) {
1217 climb_rate_cms = (packet.thrust - 0.5f) * 2.0
f *
copter.wp_nav->get_speed_up();
1220 climb_rate_cms = (0.5f - packet.thrust) * 2.0
f * -fabsf(
copter.wp_nav->get_speed_down());
1226 if ((packet.type_mask & (1<<2)) == 0) {
1227 use_yaw_rate =
true;
1230 copter.mode_guided.set_angle(
Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]),
1231 climb_rate_cms, use_yaw_rate, packet.body_yaw_rate);
1236 case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
1239 mavlink_set_position_target_local_ned_t packet;
1240 mavlink_msg_set_position_target_local_ned_decode(msg, &packet);
1243 if (!
copter.flightmode->in_guided_mode()) {
1248 if (packet.coordinate_frame != MAV_FRAME_LOCAL_NED &&
1249 packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
1250 packet.coordinate_frame != MAV_FRAME_BODY_NED &&
1251 packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
1270 pos_vector =
Vector3f(packet.x * 100.0f, packet.y * 100.0f, -packet.z * 100.0f);
1272 if (packet.coordinate_frame == MAV_FRAME_BODY_NED ||
1273 packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
1274 copter.rotate_body_frame_to_NE(pos_vector.
x, pos_vector.
y);
1277 if (packet.coordinate_frame == MAV_FRAME_LOCAL_OFFSET_NED ||
1278 packet.coordinate_frame == MAV_FRAME_BODY_NED ||
1279 packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
1280 pos_vector +=
copter.inertial_nav.get_position();
1283 pos_vector.
z =
copter.pv_alt_above_origin(pos_vector.
z);
1291 vel_vector =
Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f);
1293 if (packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
1294 copter.rotate_body_frame_to_NE(vel_vector.
x, vel_vector.
y);
1300 bool yaw_relative =
false;
1303 yaw_cd =
ToDeg(packet.yaw) * 100.0f;
1304 yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
1306 if (!yaw_rate_ignore) {
1307 yaw_rate_cds =
ToDeg(packet.yaw_rate) * 100.0f;
1311 if (!pos_ignore && !vel_ignore && acc_ignore) {
1312 if (
copter.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
1313 result = MAV_RESULT_ACCEPTED;
1315 result = MAV_RESULT_FAILED;
1317 }
else if (pos_ignore && !vel_ignore && acc_ignore) {
1318 copter.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative);
1319 result = MAV_RESULT_ACCEPTED;
1320 }
else if (!pos_ignore && vel_ignore && acc_ignore) {
1321 if (
copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
1322 result = MAV_RESULT_ACCEPTED;
1324 result = MAV_RESULT_FAILED;
1327 result = MAV_RESULT_FAILED;
1333 case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
1336 mavlink_set_position_target_global_int_t packet;
1337 mavlink_msg_set_position_target_global_int_decode(msg, &packet);
1340 if (!
copter.flightmode->in_guided_mode()) {
1345 if (packet.coordinate_frame != MAV_FRAME_GLOBAL &&
1346 packet.coordinate_frame != MAV_FRAME_GLOBAL_INT &&
1347 packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT &&
1348 packet.coordinate_frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT &&
1349 packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT &&
1350 packet.coordinate_frame != MAV_FRAME_GLOBAL_TERRAIN_ALT_INT) {
1370 result = MAV_RESULT_FAILED;
1374 loc.
lat = packet.lat_int;
1375 loc.
lng = packet.lon_int;
1376 loc.
alt = packet.alt*100;
1377 switch (packet.coordinate_frame) {
1378 case MAV_FRAME_GLOBAL_RELATIVE_ALT:
1379 case MAV_FRAME_GLOBAL_RELATIVE_ALT_INT:
1383 case MAV_FRAME_GLOBAL_TERRAIN_ALT:
1384 case MAV_FRAME_GLOBAL_TERRAIN_ALT_INT:
1388 case MAV_FRAME_GLOBAL:
1389 case MAV_FRAME_GLOBAL_INT:
1398 pos_neu_cm =
copter.pv_location_to_vector(loc);
1403 bool yaw_relative =
false;
1406 yaw_cd =
ToDeg(packet.yaw) * 100.0f;
1407 yaw_relative = packet.coordinate_frame == MAV_FRAME_BODY_NED || packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED;
1409 if (!yaw_rate_ignore) {
1410 yaw_rate_cds =
ToDeg(packet.yaw_rate) * 100.0f;
1413 if (!pos_ignore && !vel_ignore && acc_ignore) {
1414 if (
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)) {
1415 result = MAV_RESULT_ACCEPTED;
1417 result = MAV_RESULT_FAILED;
1419 }
else if (pos_ignore && !vel_ignore && acc_ignore) {
1420 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);
1421 result = MAV_RESULT_ACCEPTED;
1422 }
else if (!pos_ignore && vel_ignore && acc_ignore) {
1423 if (
copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) {
1424 result = MAV_RESULT_ACCEPTED;
1426 result = MAV_RESULT_FAILED;
1429 result = MAV_RESULT_FAILED;
1436 case MAVLINK_MSG_ID_DISTANCE_SENSOR:
1438 result = MAV_RESULT_ACCEPTED;
1439 copter.rangefinder.handle_msg(msg);
1440 #if PROXIMITY_ENABLED == ENABLED 1441 copter.g2.proximity.handle_msg(msg);
1446 #if HIL_MODE != HIL_MODE_DISABLED 1447 case MAVLINK_MSG_ID_HIL_STATE:
1449 mavlink_hil_state_t packet;
1450 mavlink_msg_hil_state_decode(msg, &packet);
1459 loc.
lat = packet.lat;
1460 loc.
lng = packet.lon;
1461 loc.
alt = packet.alt/10;
1462 Vector3f vel(packet.vx, packet.vy, packet.vz);
1466 packet.time_usec/1000,
1471 gyros.
x = packet.rollspeed;
1472 gyros.
y = packet.pitchspeed;
1473 gyros.
z = packet.yawspeed;
1486 copter.compass.setHIL(0, packet.roll, packet.pitch, packet.yaw);
1487 copter.compass.setHIL(1, packet.roll, packet.pitch, packet.yaw);
1491 #endif // HIL_MODE != HIL_MODE_DISABLED 1493 case MAVLINK_MSG_ID_RADIO:
1494 case MAVLINK_MSG_ID_RADIO_STATUS:
1500 #if PRECISION_LANDING == ENABLED 1501 case MAVLINK_MSG_ID_LANDING_TARGET:
1502 result = MAV_RESULT_ACCEPTED;
1503 copter.precland.handle_msg(msg);
1507 #if AC_FENCE == ENABLED 1509 case MAVLINK_MSG_ID_FENCE_POINT:
1510 case MAVLINK_MSG_ID_FENCE_FETCH_POINT:
1511 copter.fence.handle_msg(*
this, msg);
1513 #endif // AC_FENCE == ENABLED 1515 #if MOUNT == ENABLED 1517 case MAVLINK_MSG_ID_MOUNT_CONFIGURE:
1518 copter.camera_mount.configure_msg(msg);
1521 case MAVLINK_MSG_ID_MOUNT_CONTROL:
1522 if(!
copter.camera_mount.has_pan_control()) {
1523 copter.flightmode->auto_yaw.set_fixed_yaw(
1524 mavlink_msg_mount_control_get_input_c(msg)/100.0
f,
1529 copter.camera_mount.control_msg(msg);
1531 #endif // MOUNT == ENABLED 1533 case MAVLINK_MSG_ID_TERRAIN_DATA:
1534 case MAVLINK_MSG_ID_TERRAIN_CHECK:
1535 #if AP_TERRAIN_AVAILABLE && AC_TERRAIN 1540 case MAVLINK_MSG_ID_SET_HOME_POSITION:
1542 mavlink_set_home_position_t packet;
1543 mavlink_msg_set_home_position_decode(msg, &packet);
1544 if((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) {
1545 copter.set_home_to_current_location(
true);
1548 if (!
check_latlng(packet.latitude, packet.longitude)) {
1552 new_home_loc.
lat = packet.latitude;
1553 new_home_loc.
lng = packet.longitude;
1554 new_home_loc.
alt = packet.altitude / 10;
1555 copter.set_home(new_home_loc,
true);
1560 case MAVLINK_MSG_ID_ADSB_VEHICLE:
1561 case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG:
1562 case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
1563 case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT:
1564 #if ADSB_ENABLED == ENABLED 1569 #if TOY_MODE_ENABLED == ENABLED 1570 case MAVLINK_MSG_ID_NAMED_VALUE_INT:
1571 copter.g2.toy_mode.handle_message(msg);
1576 handle_common_message(msg);
1590 static uint32_t last_1hz, last_50hz, last_5s;
1591 if (!
gcs().
chan(0).initialised)
return;
1595 uint32_t tnow =
millis();
1596 if (tnow - last_1hz > 1000) {
1601 if (tnow - last_50hz > 20) {
1608 if (tnow - last_5s > 5000) {
1610 gcs().
send_text(MAV_SEVERITY_INFO,
"Initialising APM");
1638 if (!
copter.g2.sysid_enforce) {
1641 if (msg.msgid == MAVLINK_MSG_ID_RADIO || msg.msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
1644 return (msg.sysid ==
copter.g.sysid_my_gcs);
1649 #if MODE_AUTO_ENABLED == ENABLED 1663 #if CAMERA == ENABLED 1672 #if ADVANCED_FAILSAFE == ENABLED 1681 #if VISUAL_ODOMETRY_ENABLED == ENABLED 1682 return &
copter.g2.visual_odom;
1690 MAV_RESULT
result = MAV_RESULT_FAILED;
1692 #if ADVANCED_FAILSAFE == ENABLED 1695 if (packet.param1 > 0.5f) {
1696 copter.init_disarm_motors();
1697 result = MAV_RESULT_ACCEPTED;
1699 #if ADVANCED_FAILSAFE == ENABLED 1701 result = MAV_RESULT_ACCEPTED;
1710 #if AC_RALLY == ENABLED 1719 #ifdef DISALLOW_GCS_MODE_CHANGE_DURING_RC_FAILSAFE 1720 if (
copter.failsafe.radio) {
virtual void send_banner()
AP_Mission * get_mission() override
#define MAVLINK_SET_POS_TYPE_MASK_POS_IGNORE
bool enabled(uint8_t instance) const
const Vector3f & get_accel_ef_blended() const override
AC_AttitudeControl_t * attitude_control
void send_extended_status1(mavlink_channel_t chan)
uint8_t sysid_my_gcs() const override
#define CHECK_PAYLOAD_SIZE(id)
float get_rpm(uint8_t instance) const
const Vector3f & get_gyro(void) const override
static const ap_message STREAM_ADSB_msgs[]
AP_AdvancedFailsafe * get_advanced_failsafe() const override
bool accept_packet(const mavlink_status_t &status, mavlink_message_t &msg) override
#define MAVLINK_SET_POS_TYPE_MASK_VEL_IGNORE
void send_position_target_global_int() override
float get_alt_error() const
virtual uint8_t capacity_remaining_pct(uint8_t instance) const
static const struct stream_entries all_stream_entries[]
const AP_FWVersion & get_fwver() const override
#define AP_GROUPINFO(name, idx, clazz, element, def)
void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg) override
static const ap_message STREAM_EXTRA3_msgs[]
MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet) override
bool set_mode(uint8_t mode) override
void gcs_data_stream_send(void)
static const ap_message STREAM_RAW_SENSORS_msgs[]
#define MAV_STREAM_ENTRY(stream_name)
uint32_t control_sensors_health
void send_banner() override
bool is_positive(const T fVal1)
AP_Rally * get_rally() const override
int16_t vfr_hud_throttle() const override
MSG_NAV_CONTROLLER_OUTPUT
uint32_t telem_delay() const override
static const ap_message STREAM_EXTRA1_msgs[]
static const ap_message STREAM_EXTRA2_msgs[]
#define DATA_PARACHUTE_ENABLED
bool healthy(uint8_t instance) const
bool try_send_message(enum ap_message id) override
bool params_ready() const override
void gcs_send_heartbeat(void)
virtual int32_t wp_bearing() const
uint32_t custom_mode() const override
virtual void delay(uint16_t ms)=0
virtual MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet)
void send_message(enum ap_message id)
#define HAVE_PAYLOAD_SPACE(chan, id)
MSG_POSITION_TARGET_GLOBAL_INT
AP_Camera * get_camera() const override
#define DATA_WINCH_LENGTH_CONTROL
AP_VisualOdom * get_visual_odom() const override
#define DATA_PARACHUTE_DISABLED
bool is_zero(const T fVal1)
static const ap_message STREAM_RC_CHANNELS_msgs[]
MAV_STATE system_status() const override
bool has_current(uint8_t instance) const
static const ap_message STREAM_RAW_CONTROLLER_msgs[]
#define DATA_WINCH_RELAXED
void set_accel(uint8_t instance, const Vector3f &accel)
void send_rpm(mavlink_channel_t chan)
virtual uint32_t wp_distance() const
void set_out_of_time(bool val)
void update_sensor_status_flags(void)
void handleMessage(mavlink_message_t *msg) override
bool check_latlng(float lat, float lng)
void send_fence_status(mavlink_channel_t chan)
float current_amps(uint8_t instance) const
static const ap_message STREAM_POSITION_msgs[]
void send_text(MAV_SEVERITY severity, const char *fmt,...)
#define DATA_WINCH_RATE_CONTROL
void send_nav_controller_output(mavlink_channel_t chan)
Location_Option_Flags flags
DataFlash_Class DataFlash
uint32_t control_sensors_enabled
static const ap_message STREAM_EXTENDED_STATUS_msgs[]
void set_gyro(uint8_t instance, const Vector3f &gyro)
float constrain_float(const float amt, const float low, const float high)
void setHIL(uint8_t instance, GPS_Status status, uint64_t time_epoch_ms, const Location &location, const Vector3f &velocity, uint8_t num_sats, uint16_t hdop)
static const struct AP_Param::GroupInfo var_info[]
virtual void reboot(bool hold_in_bootloader)=0
std::enable_if< std::is_integral< typename std::common_type< Arithmetic1, Arithmetic2 >::type >::value, bool >::type is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override
static struct notify_flags_and_values_type flags
AP_HAL_MAIN_CALLBACKS & copter
AP_HAL::AnalogSource * chan
void gcs_check_input(void)
void EnableWrites(bool enable)
static bool in_sensor_config_error(void)
void fence_send_mavlink_status(mavlink_channel_t chan)
AC_PosControl * pos_control
Compass * get_compass() const override
uint32_t control_sensors_present
static void clear_overrides(void)
MAV_TYPE frame_type() const override
#define MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE
MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override
#define MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE
#define MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE
void gcs_send_deferred(void)
#define MAV_STREAM_TERMINATOR
virtual void packetReceived(const mavlink_status_t &status, mavlink_message_t &msg)
bool handle_guided_request(AP_Mission::Mission_Command &cmd) override
virtual void handle_command_ack(const mavlink_message_t *msg)
float voltage(uint8_t instance) const
bool home_is_set(void) const
static bool set_override(const uint8_t chan, const int16_t value)
virtual bool try_send_message(enum ap_message id)
void handle_command_ack(const mavlink_message_t *msg) override
AP_HAL::Scheduler * scheduler
MAV_MODE base_mode() const override
virtual MAV_RESULT handle_flight_termination(const mavlink_command_long_t &packet)
void send_pid_tuning(mavlink_channel_t chan)
void setHIL(float altitude_msl)