43 #include <airspeed/airspeed.h> 53 #include <arpa/inet.h> 54 #include <netinet/in.h> 58 #include <sys/param.h> 59 #include <sys/mount.h> 61 #include <sys/statfs.h> 73 #define MAVLINK_RECEIVER_NET_ADDED_STACK 1360 75 #define MAVLINK_RECEIVER_NET_ADDED_STACK 0 81 ModuleParams(nullptr),
84 _mavlink_log_handler(parent),
85 _mission_manager(parent),
86 _parameters_manager(parent),
87 _mavlink_timesync(parent)
98 command_ack.result = result;
99 command_ack.target_system = sysid;
100 command_ack.target_component = compid;
108 switch (msg->msgid) {
109 case MAVLINK_MSG_ID_COMMAND_LONG:
113 case MAVLINK_MSG_ID_COMMAND_INT:
117 case MAVLINK_MSG_ID_COMMAND_ACK:
121 case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
125 case MAVLINK_MSG_ID_PING:
129 case MAVLINK_MSG_ID_SET_MODE:
133 case MAVLINK_MSG_ID_ATT_POS_MOCAP:
137 case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
141 case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT:
145 case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
149 case MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET:
153 case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
157 case MAVLINK_MSG_ID_ODOMETRY:
161 case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN:
165 case MAVLINK_MSG_ID_RADIO_STATUS:
169 case MAVLINK_MSG_ID_MANUAL_CONTROL:
173 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
177 case MAVLINK_MSG_ID_HEARTBEAT:
181 case MAVLINK_MSG_ID_DISTANCE_SENSOR:
185 case MAVLINK_MSG_ID_FOLLOW_TARGET:
189 case MAVLINK_MSG_ID_LANDING_TARGET:
193 case MAVLINK_MSG_ID_CELLULAR_STATUS:
197 case MAVLINK_MSG_ID_ADSB_VEHICLE:
201 case MAVLINK_MSG_ID_UTM_GLOBAL_POSITION:
205 case MAVLINK_MSG_ID_COLLISION:
209 case MAVLINK_MSG_ID_GPS_RTCM_DATA:
213 case MAVLINK_MSG_ID_BATTERY_STATUS:
217 case MAVLINK_MSG_ID_SERIAL_CONTROL:
221 case MAVLINK_MSG_ID_LOGGING_ACK:
225 case MAVLINK_MSG_ID_PLAY_TUNE:
229 case MAVLINK_MSG_ID_OBSTACLE_DISTANCE:
233 case MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS:
237 case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT:
241 case MAVLINK_MSG_ID_DEBUG:
245 case MAVLINK_MSG_ID_DEBUG_VECT:
249 case MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY:
253 case MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS:
257 case MAVLINK_MSG_ID_STATUSTEXT:
276 switch (msg->msgid) {
277 case MAVLINK_MSG_ID_HIL_SENSOR:
281 case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
285 case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
296 switch (msg->msgid) {
297 case MAVLINK_MSG_ID_HIL_GPS:
316 bool target_ok =
false;
320 case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES:
321 case MAV_CMD_REQUEST_PROTOCOL_VERSION:
323 target_ok = (target_system == 0) || (target_system ==
mavlink_system.sysid);
328 || (target_component == MAV_COMP_ID_ALL));
338 mavlink_flight_information_t flight_info{};
339 flight_info.flight_uuid =
static_cast<uint64_t
>(_param_com_flight_uuid.get());
344 if (ret && actuator_armed.timestamp != 0) {
345 flight_info.arming_time_utc = flight_info.takeoff_time_utc = actuator_armed.armed_time_ms;
355 mavlink_storage_information_t storage_info{};
356 const char *microsd_dir = PX4_STORAGEDIR;
358 if (storage_id == 0 || storage_id == 1) {
359 storage_info.storage_id = 1;
361 struct statfs statfs_buf;
362 uint64_t total_bytes = 0;
363 uint64_t avail_bytes = 0;
365 if (statfs(microsd_dir, &statfs_buf) == 0) {
366 total_bytes = (uint64_t)statfs_buf.f_blocks * statfs_buf.f_bsize;
367 avail_bytes = (uint64_t)statfs_buf.f_bavail * statfs_buf.f_bsize;
370 if (total_bytes == 0) {
371 storage_info.storage_count = 0;
372 storage_info.status = 0;
375 storage_info.storage_count = 1;
376 storage_info.status = 2;
377 storage_info.total_capacity = total_bytes / 1024. / 1024.;
378 storage_info.available_capacity = avail_bytes / 1024. / 1024.;
379 storage_info.used_capacity = (total_bytes - avail_bytes) / 1024. / 1024.;
384 storage_info.storage_id = storage_id;
385 storage_info.storage_count = 1;
396 mavlink_command_long_t cmd_mavlink;
397 mavlink_msg_command_long_decode(msg, &cmd_mavlink);
404 vcmd.param1 = cmd_mavlink.param1;
405 vcmd.param2 = cmd_mavlink.param2;
406 vcmd.param3 = cmd_mavlink.param3;
407 vcmd.param4 = cmd_mavlink.param4;
408 vcmd.param5 = (double)cmd_mavlink.param5;
409 vcmd.param6 = (
double)cmd_mavlink.param6;
410 vcmd.param7 = cmd_mavlink.param7;
411 vcmd.command = cmd_mavlink.command;
412 vcmd.target_system = cmd_mavlink.target_system;
413 vcmd.target_component = cmd_mavlink.target_component;
414 vcmd.source_system = msg->sysid;
415 vcmd.source_component = msg->compid;
416 vcmd.confirmation = cmd_mavlink.confirmation;
417 vcmd.from_external =
true;
426 mavlink_command_int_t cmd_mavlink;
427 mavlink_msg_command_int_decode(msg, &cmd_mavlink);
433 vcmd.param1 = cmd_mavlink.param1;
434 vcmd.param2 = cmd_mavlink.param2;
435 vcmd.param3 = cmd_mavlink.param3;
436 vcmd.param4 = cmd_mavlink.param4;
437 vcmd.param5 = ((double)cmd_mavlink.x) / 1e7;
438 vcmd.param6 = ((double)cmd_mavlink.y) / 1e7;
439 vcmd.param7 = cmd_mavlink.z;
440 vcmd.command = cmd_mavlink.command;
441 vcmd.target_system = cmd_mavlink.target_system;
442 vcmd.target_component = cmd_mavlink.target_component;
443 vcmd.source_system = msg->sysid;
444 vcmd.source_component = msg->compid;
445 vcmd.confirmation =
false;
446 vcmd.from_external =
true;
455 bool target_ok =
evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
457 bool send_ack =
true;
458 uint8_t result = vehicle_command_ack_s::VEHICLE_RESULT_ACCEPTED;
461 acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, vehicle_command_ack_s::VEHICLE_RESULT_FAILED);
465 if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
469 }
else if (cmd_mavlink.command == MAV_CMD_REQUEST_PROTOCOL_VERSION) {
473 }
else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
476 }
else if (cmd_mavlink.command == MAV_CMD_SET_MESSAGE_INTERVAL) {
477 if (
set_message_interval((
int)(cmd_mavlink.param1 + 0.5f), cmd_mavlink.param2, cmd_mavlink.param3)) {
478 result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
481 }
else if (cmd_mavlink.command == MAV_CMD_GET_MESSAGE_INTERVAL) {
484 }
else if (cmd_mavlink.command == MAV_CMD_REQUEST_FLIGHT_INFORMATION) {
487 }
else if (cmd_mavlink.command == MAV_CMD_REQUEST_STORAGE_INFORMATION) {
488 if ((
int)(cmd_mavlink.param2 + 0.5f) == 1) {
501 if (cmd_mavlink.command == MAV_CMD_LOGGING_START) {
507 result = vehicle_command_ack_s::VEHICLE_RESULT_DENIED;
518 }
else if (cmd_mavlink.command == MAV_CMD_LOGGING_STOP) {
528 acknowledge(msg->sysid, msg->compid, cmd_mavlink.command, result);
535 mavlink_command_ack_t ack;
536 mavlink_msg_command_ack_decode(msg, &ack);
543 command_ack.result_param2 = ack.result_param2;
544 command_ack.command = ack.command;
545 command_ack.result = ack.result;
546 command_ack.from_external =
true;
547 command_ack.result_param1 = ack.progress;
548 command_ack.target_system = ack.target_system;
549 command_ack.target_component = ack.target_component;
554 if (ack.result != MAV_RESULT_ACCEPTED && ack.result != MAV_RESULT_IN_PROGRESS) {
555 if (msg->compid == MAV_COMP_ID_CAMERA) {
556 PX4_WARN(
"Got unsuccessful result %d from camera", ack.result);
565 mavlink_optical_flow_rad_t flow;
566 mavlink_msg_optical_flow_rad_decode(msg, &flow);
571 f.time_since_last_sonar_update = flow.time_delta_distance_us;
572 f.integration_timespan = flow.integration_time_us;
573 f.pixel_flow_x_integral = flow.integrated_x;
574 f.pixel_flow_y_integral = flow.integrated_y;
575 f.gyro_x_rate_integral = flow.integrated_xgyro;
576 f.gyro_y_rate_integral = flow.integrated_ygyro;
577 f.gyro_z_rate_integral = flow.integrated_zgyro;
578 f.gyro_temperature = flow.temperature;
579 f.ground_distance_m = flow.distance;
580 f.quality = flow.quality;
581 f.sensor_id = flow.sensor_id;
582 f.max_flow_rate = _param_sens_flow_maxr.get();
583 f.min_ground_distance = _param_sens_flow_minhgt.get();
584 f.max_ground_distance = _param_sens_flow_maxhgt.get();
590 float zero_val = 0.0f;
591 rotate_3f(flow_rot,
f.pixel_flow_x_integral,
f.pixel_flow_y_integral, zero_val);
592 rotate_3f(flow_rot,
f.gyro_x_rate_integral,
f.gyro_y_rate_integral,
f.gyro_z_rate_integral);
597 if (flow.distance > 0.0f) {
602 d.min_distance = 0.3f;
603 d.max_distance = 5.0f;
604 d.current_distance = flow.distance;
606 d.id = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
607 d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
618 mavlink_hil_optical_flow_t flow;
619 mavlink_msg_hil_optical_flow_decode(msg, &flow);
624 f.integration_timespan = flow.integration_time_us;
625 f.pixel_flow_x_integral = flow.integrated_x;
626 f.pixel_flow_y_integral = flow.integrated_y;
627 f.gyro_x_rate_integral = flow.integrated_xgyro;
628 f.gyro_y_rate_integral = flow.integrated_ygyro;
629 f.gyro_z_rate_integral = flow.integrated_zgyro;
630 f.time_since_last_sonar_update = flow.time_delta_distance_us;
631 f.ground_distance_m = flow.distance;
632 f.quality = flow.quality;
633 f.sensor_id = flow.sensor_id;
634 f.gyro_temperature = flow.temperature;
642 d.min_distance = 0.3f;
643 d.max_distance = 5.0f;
644 d.current_distance = flow.distance;
645 d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
647 d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
656 mavlink_set_mode_t new_mode;
657 mavlink_msg_set_mode_decode(msg, &new_mode);
660 custom_mode.
data = new_mode.custom_mode;
667 vcmd.param1 = (float)new_mode.base_mode;
668 vcmd.param2 = (
float)custom_mode.
main_mode;
669 vcmd.param3 = (float)custom_mode.
sub_mode;
671 vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
672 vcmd.target_system = new_mode.target_system;
673 vcmd.target_component = MAV_COMP_ID_ALL;
674 vcmd.source_system = msg->sysid;
675 vcmd.source_component = msg->compid;
676 vcmd.confirmation =
true;
677 vcmd.from_external =
true;
685 mavlink_distance_sensor_t dist_sensor;
686 mavlink_msg_distance_sensor_decode(msg, &dist_sensor);
691 ds.min_distance =
static_cast<float>(dist_sensor.min_distance) * 1e-2
f;
692 ds.max_distance =
static_cast<float>(dist_sensor.max_distance) * 1e-2
f;
693 ds.current_distance =
static_cast<float>(dist_sensor.current_distance) * 1e-2
f;
694 ds.variance = dist_sensor.covariance * 1e-4
f;
695 ds.h_fov = dist_sensor.horizontal_fov;
696 ds.v_fov = dist_sensor.vertical_fov;
697 ds.q[0] = dist_sensor.quaternion[0];
698 ds.q[1] = dist_sensor.quaternion[1];
699 ds.q[2] = dist_sensor.quaternion[2];
700 ds.q[3] = dist_sensor.quaternion[3];
701 ds.signal_quality = -1;
702 ds.type = dist_sensor.type;
703 ds.id = MAV_DISTANCE_SENSOR_LASER;
704 ds.orientation = dist_sensor.orientation;
712 mavlink_att_pos_mocap_t mocap;
713 mavlink_msg_att_pos_mocap_decode(msg, &mocap);
718 mocap_odom.x = mocap.x;
719 mocap_odom.y = mocap.y;
720 mocap_odom.z = mocap.z;
721 mocap_odom.q[0] = mocap.q[0];
722 mocap_odom.q[1] = mocap.q[1];
723 mocap_odom.q[2] = mocap.q[2];
724 mocap_odom.q[3] = mocap.q[3];
726 const size_t URT_SIZE =
sizeof(mocap_odom.pose_covariance) /
sizeof(mocap_odom.pose_covariance[0]);
727 static_assert(URT_SIZE == (
sizeof(mocap.covariance) /
sizeof(mocap.covariance[0])),
728 "Odometry Pose Covariance matrix URT array size mismatch");
730 for (
size_t i = 0; i < URT_SIZE; i++) {
731 mocap_odom.pose_covariance[i] = mocap.covariance[i];
737 mocap_odom.rollspeed = NAN;
738 mocap_odom.pitchspeed = NAN;
739 mocap_odom.yawspeed = NAN;
740 mocap_odom.velocity_covariance[0] = NAN;
748 mavlink_set_position_target_local_ned_t set_position_target_local_ned;
749 mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
751 const bool values_finite =
752 PX4_ISFINITE(set_position_target_local_ned.x) &&
753 PX4_ISFINITE(set_position_target_local_ned.y) &&
754 PX4_ISFINITE(set_position_target_local_ned.z) &&
755 PX4_ISFINITE(set_position_target_local_ned.vx) &&
756 PX4_ISFINITE(set_position_target_local_ned.vy) &&
757 PX4_ISFINITE(set_position_target_local_ned.vz) &&
758 PX4_ISFINITE(set_position_target_local_ned.afx) &&
759 PX4_ISFINITE(set_position_target_local_ned.afy) &&
760 PX4_ISFINITE(set_position_target_local_ned.afz) &&
761 PX4_ISFINITE(set_position_target_local_ned.yaw);
764 if ((
mavlink_system.sysid == set_position_target_local_ned.target_system ||
765 set_position_target_local_ned.target_system == 0) &&
766 (
mavlink_system.compid == set_position_target_local_ned.target_component ||
767 set_position_target_local_ned.target_component == 0) &&
773 offboard_control_mode.
ignore_position = (bool)(set_position_target_local_ned.type_mask & 0x7);
774 offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_local_ned.type_mask & 0x4);
775 offboard_control_mode.ignore_velocity = (bool)(set_position_target_local_ned.type_mask & 0x38);
776 offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_local_ned.type_mask & 0x1C0);
777 offboard_control_mode.ignore_attitude = (bool)(set_position_target_local_ned.type_mask & 0x400);
778 offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_local_ned.type_mask & 0x800);
779 offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_local_ned.type_mask & 0x800);
780 offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_local_ned.type_mask & 0x800);
783 bool is_force_sp = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
785 bool is_takeoff_sp = (bool)(set_position_target_local_ned.type_mask & 0x1000);
786 bool is_land_sp = (bool)(set_position_target_local_ned.type_mask & 0x2000);
787 bool is_loiter_sp = (bool)(set_position_target_local_ned.type_mask & 0x3000);
788 bool is_idle_sp = (bool)(set_position_target_local_ned.type_mask & 0x4000);
800 if (control_mode.flag_control_offboard_enabled) {
801 if (is_force_sp && offboard_control_mode.ignore_position &&
802 offboard_control_mode.ignore_velocity) {
804 PX4_WARN(
"force setpoint not supported");
811 pos_sp_triplet.previous.valid =
false;
812 pos_sp_triplet.next.valid =
false;
813 pos_sp_triplet.current.valid =
true;
818 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
820 }
else if (is_takeoff_sp) {
821 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
823 }
else if (is_land_sp) {
824 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
826 }
else if (is_idle_sp) {
827 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
830 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
834 if (!offboard_control_mode.ignore_position) {
836 pos_sp_triplet.current.position_valid =
true;
837 pos_sp_triplet.current.x = set_position_target_local_ned.x;
838 pos_sp_triplet.current.y = set_position_target_local_ned.y;
839 pos_sp_triplet.current.z = set_position_target_local_ned.z;
842 pos_sp_triplet.current.position_valid =
false;
846 if (!offboard_control_mode.ignore_velocity) {
848 pos_sp_triplet.current.velocity_valid =
true;
849 pos_sp_triplet.current.vx = set_position_target_local_ned.vx;
850 pos_sp_triplet.current.vy = set_position_target_local_ned.vy;
851 pos_sp_triplet.current.vz = set_position_target_local_ned.vz;
853 pos_sp_triplet.current.velocity_frame = set_position_target_local_ned.coordinate_frame;
856 pos_sp_triplet.current.velocity_valid =
false;
859 if (!offboard_control_mode.ignore_alt_hold) {
860 pos_sp_triplet.current.alt_valid =
true;
861 pos_sp_triplet.current.z = set_position_target_local_ned.z;
864 pos_sp_triplet.current.alt_valid =
false;
869 if (!offboard_control_mode.ignore_acceleration_force) {
871 pos_sp_triplet.current.acceleration_valid =
true;
872 pos_sp_triplet.current.a_x = set_position_target_local_ned.afx;
873 pos_sp_triplet.current.a_y = set_position_target_local_ned.afy;
874 pos_sp_triplet.current.a_z = set_position_target_local_ned.afz;
875 pos_sp_triplet.current.acceleration_is_force = is_force_sp;
878 pos_sp_triplet.current.acceleration_valid =
false;
882 if (!offboard_control_mode.ignore_attitude) {
883 pos_sp_triplet.current.yaw_valid =
true;
884 pos_sp_triplet.current.yaw = set_position_target_local_ned.yaw;
887 pos_sp_triplet.current.yaw_valid =
false;
891 if (!(offboard_control_mode.ignore_bodyrate_x ||
892 offboard_control_mode.ignore_bodyrate_y ||
893 offboard_control_mode.ignore_bodyrate_z)) {
895 pos_sp_triplet.current.yawspeed_valid =
true;
896 pos_sp_triplet.current.yawspeed = set_position_target_local_ned.yaw_rate;
899 pos_sp_triplet.current.yawspeed_valid =
false;
913 mavlink_set_position_target_global_int_t set_position_target_global_int;
914 mavlink_msg_set_position_target_global_int_decode(msg, &set_position_target_global_int);
916 const bool values_finite =
917 PX4_ISFINITE(set_position_target_global_int.alt) &&
918 PX4_ISFINITE(set_position_target_global_int.vx) &&
919 PX4_ISFINITE(set_position_target_global_int.vy) &&
920 PX4_ISFINITE(set_position_target_global_int.vz) &&
921 PX4_ISFINITE(set_position_target_global_int.afx) &&
922 PX4_ISFINITE(set_position_target_global_int.afy) &&
923 PX4_ISFINITE(set_position_target_global_int.afz) &&
924 PX4_ISFINITE(set_position_target_global_int.yaw);
927 if ((
mavlink_system.sysid == set_position_target_global_int.target_system ||
928 set_position_target_global_int.target_system == 0) &&
929 (
mavlink_system.compid == set_position_target_global_int.target_component ||
930 set_position_target_global_int.target_component == 0) &&
936 offboard_control_mode.
ignore_position = (bool)(set_position_target_global_int.type_mask &
937 (POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_X_IGNORE
938 | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Y_IGNORE
939 | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_Z_IGNORE));
940 offboard_control_mode.ignore_alt_hold = (bool)(set_position_target_global_int.type_mask & 0x4);
941 offboard_control_mode.ignore_velocity = (bool)(set_position_target_global_int.type_mask &
942 (POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VX_IGNORE
943 | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VY_IGNORE
944 | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_VZ_IGNORE));
945 offboard_control_mode.ignore_acceleration_force = (bool)(set_position_target_global_int.type_mask &
946 (POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AX_IGNORE
947 | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AY_IGNORE
948 | POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_AZ_IGNORE));
950 offboard_control_mode.ignore_attitude = (bool)(set_position_target_global_int.type_mask &
951 POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_IGNORE);
952 offboard_control_mode.ignore_bodyrate_x = (bool)(set_position_target_global_int.type_mask &
953 POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
954 offboard_control_mode.ignore_bodyrate_y = (bool)(set_position_target_global_int.type_mask &
955 POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
956 offboard_control_mode.ignore_bodyrate_z = (bool)(set_position_target_global_int.type_mask &
957 POSITION_TARGET_TYPEMASK::POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE);
959 bool is_force_sp = (bool)(set_position_target_global_int.type_mask & (1 << 9));
971 if (control_mode.flag_control_offboard_enabled) {
972 if (is_force_sp && offboard_control_mode.ignore_position &&
973 offboard_control_mode.ignore_velocity) {
975 PX4_WARN(
"force setpoint not supported");
982 pos_sp_triplet.previous.valid =
false;
983 pos_sp_triplet.next.valid =
false;
984 pos_sp_triplet.current.valid =
true;
988 if (set_position_target_global_int.type_mask & 0x3000) {
989 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
991 }
else if (set_position_target_global_int.type_mask & 0x1000) {
992 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;
994 }
else if (set_position_target_global_int.type_mask & 0x2000) {
995 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_LAND;
997 }
else if (set_position_target_global_int.type_mask & 0x4000) {
998 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
1001 pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
1010 local_pos.ref_alt, local_pos.ref_timestamp);
1011 pos_sp_triplet.current.position_valid =
false;
1015 set_position_target_global_int.lon_int / 1e7, set_position_target_global_int.alt,
1016 &pos_sp_triplet.current.x, &pos_sp_triplet.current.y, &pos_sp_triplet.current.z);
1017 pos_sp_triplet.current.position_valid =
true;
1021 pos_sp_triplet.current.position_valid =
false;
1025 if (!offboard_control_mode.ignore_velocity) {
1027 pos_sp_triplet.current.velocity_valid =
true;
1028 pos_sp_triplet.current.vx = set_position_target_global_int.vx;
1029 pos_sp_triplet.current.vy = set_position_target_global_int.vy;
1030 pos_sp_triplet.current.vz = set_position_target_global_int.vz;
1032 pos_sp_triplet.current.velocity_frame = set_position_target_global_int.coordinate_frame;
1035 pos_sp_triplet.current.velocity_valid =
false;
1038 if (!offboard_control_mode.ignore_alt_hold) {
1039 pos_sp_triplet.current.alt_valid =
true;
1042 pos_sp_triplet.current.alt_valid =
false;
1047 if (!offboard_control_mode.ignore_acceleration_force) {
1049 pos_sp_triplet.current.acceleration_valid =
true;
1050 pos_sp_triplet.current.a_x = set_position_target_global_int.afx;
1051 pos_sp_triplet.current.a_y = set_position_target_global_int.afy;
1052 pos_sp_triplet.current.a_z = set_position_target_global_int.afz;
1053 pos_sp_triplet.current.acceleration_is_force = is_force_sp;
1056 pos_sp_triplet.current.acceleration_valid =
false;
1060 if (!offboard_control_mode.ignore_attitude) {
1061 pos_sp_triplet.current.yaw_valid =
true;
1062 pos_sp_triplet.current.yaw = set_position_target_global_int.yaw;
1065 pos_sp_triplet.current.yaw_valid =
false;
1069 if (!(offboard_control_mode.ignore_bodyrate_x ||
1070 offboard_control_mode.ignore_bodyrate_y ||
1071 offboard_control_mode.ignore_bodyrate_z)) {
1073 pos_sp_triplet.current.yawspeed_valid =
true;
1074 pos_sp_triplet.current.yawspeed = set_position_target_global_int.yaw_rate;
1077 pos_sp_triplet.current.yawspeed_valid =
false;
1090 mavlink_set_actuator_control_target_t set_actuator_control_target;
1091 mavlink_msg_set_actuator_control_target_decode(msg, &set_actuator_control_target);
1093 bool values_finite =
1094 PX4_ISFINITE(set_actuator_control_target.controls[0]) &&
1095 PX4_ISFINITE(set_actuator_control_target.controls[1]) &&
1096 PX4_ISFINITE(set_actuator_control_target.controls[2]) &&
1097 PX4_ISFINITE(set_actuator_control_target.controls[3]) &&
1098 PX4_ISFINITE(set_actuator_control_target.controls[4]) &&
1099 PX4_ISFINITE(set_actuator_control_target.controls[5]) &&
1100 PX4_ISFINITE(set_actuator_control_target.controls[6]) &&
1101 PX4_ISFINITE(set_actuator_control_target.controls[7]);
1103 if ((
mavlink_system.sysid == set_actuator_control_target.target_system ||
1104 set_actuator_control_target.target_system == 0) &&
1105 (
mavlink_system.compid == set_actuator_control_target.target_component ||
1106 set_actuator_control_target.target_component == 0) &&
1109 #if defined(ENABLE_LOCKSTEP_SCHEDULER) 1110 PX4_ERR(
"SET_ACTUATOR_CONTROL_TARGET not supported with lockstep enabled");
1111 PX4_ERR(
"Please disable lockstep for actuator offboard control:");
1112 PX4_ERR(
"https://dev.px4.io/master/en/simulation/#disable-lockstep-simulation");
1116 bool ignore_setpoints = bool(set_actuator_control_target.group_mlx != 2);
1121 offboard_control_mode.ignore_attitude = ignore_setpoints;
1122 offboard_control_mode.ignore_bodyrate_x = ignore_setpoints;
1123 offboard_control_mode.ignore_bodyrate_y = ignore_setpoints;
1124 offboard_control_mode.ignore_bodyrate_z = ignore_setpoints;
1125 offboard_control_mode.ignore_position = ignore_setpoints;
1126 offboard_control_mode.ignore_velocity = ignore_setpoints;
1127 offboard_control_mode.ignore_acceleration_force = ignore_setpoints;
1137 if (control_mode.flag_control_offboard_enabled) {
1143 for (
size_t i = 0; i < 8; i++) {
1147 switch (set_actuator_control_target.group_mlx) {
1175 mavlink_gps_global_origin_t origin;
1176 mavlink_msg_gps_global_origin_decode(msg, &origin);
1190 mavlink_vision_position_estimate_t ev;
1191 mavlink_msg_vision_position_estimate_decode(msg, &ev);
1196 visual_odom.x = ev.x;
1197 visual_odom.y = ev.y;
1198 visual_odom.z = ev.z;
1206 visual_odom.local_frame = visual_odom.LOCAL_FRAME_NED;
1208 const size_t URT_SIZE =
sizeof(visual_odom.pose_covariance) /
sizeof(visual_odom.pose_covariance[0]);
1209 static_assert(URT_SIZE == (
sizeof(ev.covariance) /
sizeof(ev.covariance[0])),
1210 "Odometry Pose Covariance matrix URT array size mismatch");
1212 for (
size_t i = 0; i < URT_SIZE; i++) {
1213 visual_odom.pose_covariance[i] = ev.covariance[i];
1216 visual_odom.vx = NAN;
1217 visual_odom.vy = NAN;
1218 visual_odom.vz = NAN;
1219 visual_odom.rollspeed = NAN;
1220 visual_odom.pitchspeed = NAN;
1221 visual_odom.yawspeed = NAN;
1222 visual_odom.velocity_covariance[0] = NAN;
1230 mavlink_odometry_t odom;
1231 mavlink_msg_odometry_decode(msg, &odom);
1238 odometry.x = odom.x;
1239 odometry.y = odom.y;
1240 odometry.z = odom.z;
1246 q_body_to_local.
copyTo(odometry.q);
1252 odometry.local_frame = odometry.LOCAL_FRAME_NED;
1255 static constexpr
size_t POS_URT_SIZE =
sizeof(odometry.pose_covariance) /
sizeof(odometry.pose_covariance[0]);
1256 static_assert(POS_URT_SIZE == (
sizeof(odom.pose_covariance) /
sizeof(odom.pose_covariance[0])),
1257 "Odometry Pose Covariance matrix URT array size mismatch");
1260 static constexpr
size_t VEL_URT_SIZE =
sizeof(odometry.velocity_covariance) /
sizeof(odometry.velocity_covariance[0]);
1261 static_assert(VEL_URT_SIZE == (
sizeof(odom.velocity_covariance) /
sizeof(odom.velocity_covariance[0])),
1262 "Odometry Velocity Covariance matrix URT array size mismatch");
1265 for (
size_t i = 0; i < POS_URT_SIZE; i++) {
1266 odometry.pose_covariance[i] = odom.pose_covariance[i];
1275 if (odom.child_frame_id == MAV_FRAME_BODY_FRD) {
1283 odometry.vx = linvel_local(0);
1284 odometry.vy = linvel_local(1);
1285 odometry.vz = linvel_local(2);
1287 odometry.rollspeed = odom.rollspeed;
1288 odometry.pitchspeed = odom.pitchspeed;
1289 odometry.yawspeed = odom.yawspeed;
1292 for (
size_t i = 0; i < VEL_URT_SIZE; i++) {
1293 odometry.velocity_covariance[i] = odom.velocity_covariance[i];
1297 PX4_ERR(
"Body frame %u not supported. Unable to publish velocity", odom.child_frame_id);
1300 if (odom.frame_id == MAV_FRAME_LOCAL_FRD) {
1303 }
else if (odom.frame_id == MAV_FRAME_MOCAP_NED) {
1307 PX4_ERR(
"Local frame %u not supported. Unable to publish pose and velocity", odom.frame_id);
1316 case MAV_TYPE_GENERIC:
1319 case MAV_TYPE_FIXED_WING:
1320 case MAV_TYPE_GROUND_ROVER:
1321 thrust_body_array[0] = thrust;
1324 case MAV_TYPE_QUADROTOR:
1325 case MAV_TYPE_HEXAROTOR:
1326 case MAV_TYPE_OCTOROTOR:
1327 case MAV_TYPE_TRICOPTER:
1328 case MAV_TYPE_HELICOPTER:
1329 thrust_body_array[2] = -thrust;
1332 case MAV_TYPE_VTOL_DUOROTOR:
1333 case MAV_TYPE_VTOL_QUADROTOR:
1334 case MAV_TYPE_VTOL_TILTROTOR:
1335 case MAV_TYPE_VTOL_RESERVED2:
1336 case MAV_TYPE_VTOL_RESERVED3:
1337 case MAV_TYPE_VTOL_RESERVED4:
1338 case MAV_TYPE_VTOL_RESERVED5:
1339 switch (vehicle_type) {
1341 thrust_body_array[0] = thrust;
1345 case vehicle_status_s::VEHICLE_TYPE_ROTARY_WING:
1346 thrust_body_array[2] = -thrust;
1362 mavlink_set_attitude_target_t set_attitude_target;
1363 mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
1365 bool values_finite =
1366 PX4_ISFINITE(set_attitude_target.q[0]) &&
1367 PX4_ISFINITE(set_attitude_target.q[1]) &&
1368 PX4_ISFINITE(set_attitude_target.q[2]) &&
1369 PX4_ISFINITE(set_attitude_target.q[3]) &&
1370 PX4_ISFINITE(set_attitude_target.thrust) &&
1371 PX4_ISFINITE(set_attitude_target.body_roll_rate) &&
1372 PX4_ISFINITE(set_attitude_target.body_pitch_rate) &&
1373 PX4_ISFINITE(set_attitude_target.body_yaw_rate);
1376 if ((
mavlink_system.sysid == set_attitude_target.target_system ||
1377 set_attitude_target.target_system == 0) &&
1378 (
mavlink_system.compid == set_attitude_target.target_component ||
1379 set_attitude_target.target_component == 0) &&
1385 offboard_control_mode.
ignore_thrust = (bool)(set_attitude_target.type_mask & (1 << 6));
1399 bool ignore_bodyrate_msg_x = (bool)(set_attitude_target.type_mask & 0x1);
1400 bool ignore_bodyrate_msg_y = (bool)(set_attitude_target.type_mask & 0x2);
1401 bool ignore_bodyrate_msg_z = (bool)(set_attitude_target.type_mask & 0x4);
1402 bool ignore_attitude_msg = (bool)(set_attitude_target.type_mask & (1 << 7));
1405 if ((ignore_bodyrate_msg_x || ignore_bodyrate_msg_y ||
1406 ignore_bodyrate_msg_z) &&
1407 ignore_attitude_msg && !offboard_control_mode.ignore_thrust) {
1410 offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x && offboard_control_mode.ignore_bodyrate_x;
1411 offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y && offboard_control_mode.ignore_bodyrate_y;
1412 offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z && offboard_control_mode.ignore_bodyrate_z;
1413 offboard_control_mode.ignore_attitude = ignore_attitude_msg && offboard_control_mode.ignore_attitude;
1416 offboard_control_mode.ignore_bodyrate_x = ignore_bodyrate_msg_x;
1417 offboard_control_mode.ignore_bodyrate_y = ignore_bodyrate_msg_y;
1418 offboard_control_mode.ignore_bodyrate_z = ignore_bodyrate_msg_z;
1419 offboard_control_mode.ignore_attitude = ignore_attitude_msg;
1422 offboard_control_mode.ignore_position =
true;
1423 offboard_control_mode.ignore_velocity =
true;
1424 offboard_control_mode.ignore_acceleration_force =
true;
1437 if (control_mode.flag_control_offboard_enabled) {
1442 if (!(offboard_control_mode.ignore_attitude)) {
1446 if (!ignore_attitude_msg) {
1458 if (!offboard_control_mode.ignore_thrust) {
1463 if (vehicle_status.is_vtol && (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
1475 if (!offboard_control_mode.ignore_bodyrate_x ||
1476 !offboard_control_mode.ignore_bodyrate_y ||
1477 !offboard_control_mode.ignore_bodyrate_z) {
1484 if (!ignore_bodyrate_msg_x) {
1485 rates_sp.roll = set_attitude_target.body_roll_rate;
1488 if (!ignore_bodyrate_msg_y) {
1489 rates_sp.pitch = set_attitude_target.body_pitch_rate;
1492 if (!ignore_bodyrate_msg_z) {
1493 rates_sp.yaw = set_attitude_target.body_yaw_rate;
1496 if (!offboard_control_mode.ignore_thrust) {
1497 fill_thrust(rates_sp.thrust_body, vehicle_status.vehicle_type, set_attitude_target.thrust);
1512 mavlink_radio_status_t rstatus;
1513 mavlink_msg_radio_status_decode(msg, &rstatus);
1518 status.rssi = rstatus.rssi;
1519 status.remote_rssi = rstatus.remrssi;
1520 status.txbuf = rstatus.txbuf;
1521 status.noise = rstatus.noise;
1522 status.remote_noise = rstatus.remnoise;
1523 status.rxerrors = rstatus.rxerrors;
1524 status.fix = rstatus.fixed;
1535 mavlink_ping_t ping;
1536 mavlink_msg_ping_decode(msg, &ping);
1538 if ((ping.target_system == 0) &&
1539 (ping.target_component == 0)) {
1541 ping.target_system = msg->sysid;
1542 ping.target_component = msg->compid;
1546 (ping.target_component ==
1552 float rtt_ms = (now - ping.time_usec) / 1000.0
f;
1585 uorb_ping_msg.ping_time = ping.time_usec;
1586 uorb_ping_msg.ping_sequence = ping.seq;
1588 uorb_ping_msg.rtt_ms = rtt_ms;
1589 uorb_ping_msg.system_id = msg->sysid;
1590 uorb_ping_msg.component_id = msg->compid;
1606 mavlink_battery_status_t battery_mavlink;
1607 mavlink_msg_battery_status_decode(msg, &battery_mavlink);
1612 float voltage_sum = 0.0f;
1613 uint8_t cell_count = 0;
1615 while (battery_mavlink.voltages[cell_count] < UINT16_MAX && cell_count < 10) {
1616 voltage_sum += (float)(battery_mavlink.voltages[cell_count]) / 1000.0f;
1624 battery_status.remaining = (float)battery_mavlink.battery_remaining / 100.0f;
1625 battery_status.discharged_mah = (
float)battery_mavlink.current_consumed;
1632 battery_status.warning = battery_status_s::BATTERY_WARNING_EMERGENCY;
1634 }
else if (
battery_status.remaining < _param_bat_crit_thr.get()) {
1635 battery_status.warning = battery_status_s::BATTERY_WARNING_CRITICAL;
1637 }
else if (
battery_status.remaining < _param_bat_low_thr.get()) {
1647 mavlink_serial_control_t serial_control_mavlink;
1648 mavlink_msg_serial_control_decode(msg, &serial_control_mavlink);
1651 if (serial_control_mavlink.device != SERIAL_CONTROL_DEV_SHELL
1652 || (serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_REPLY)) {
1660 if (serial_control_mavlink.count > 0) {
1661 shell->
write(serial_control_mavlink.data, serial_control_mavlink.count);
1665 if ((serial_control_mavlink.flags & SERIAL_CONTROL_FLAG_RESPOND) == 0) {
1674 mavlink_logging_ack_t logging_ack;
1675 mavlink_msg_logging_ack_decode(msg, &logging_ack);
1679 if (ulog_streaming) {
1687 mavlink_play_tune_t play_tune;
1688 mavlink_msg_play_tune_decode(msg, &play_tune);
1690 char *tune = play_tune.tune;
1693 play_tune.target_system == 0) &&
1695 play_tune.target_component == 0)) {
1711 mavlink_obstacle_distance_t mavlink_obstacle_distance;
1712 mavlink_msg_obstacle_distance_decode(msg, &mavlink_obstacle_distance);
1717 obstacle_distance.sensor_type = mavlink_obstacle_distance.sensor_type;
1718 memcpy(obstacle_distance.distances, mavlink_obstacle_distance.distances,
sizeof(obstacle_distance.distances));
1720 if (mavlink_obstacle_distance.increment_f > 0.f) {
1721 obstacle_distance.increment = mavlink_obstacle_distance.increment_f;
1724 obstacle_distance.increment = (float)mavlink_obstacle_distance.increment;
1727 obstacle_distance.min_distance = mavlink_obstacle_distance.min_distance;
1728 obstacle_distance.max_distance = mavlink_obstacle_distance.max_distance;
1729 obstacle_distance.angle_offset = mavlink_obstacle_distance.angle_offset;
1730 obstacle_distance.frame = mavlink_obstacle_distance.frame;
1738 mavlink_trajectory_representation_waypoints_t trajectory;
1739 mavlink_msg_trajectory_representation_waypoints_decode(msg, &trajectory);
1744 const int number_valid_points = trajectory.valid_points;
1746 for (
int i = 0; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
1747 trajectory_waypoint.waypoints[i].position[0] = trajectory.pos_x[i];
1748 trajectory_waypoint.waypoints[i].position[1] = trajectory.pos_y[i];
1749 trajectory_waypoint.waypoints[i].position[2] = trajectory.pos_z[i];
1751 trajectory_waypoint.waypoints[i].velocity[0] = trajectory.vel_x[i];
1752 trajectory_waypoint.waypoints[i].velocity[1] = trajectory.vel_y[i];
1753 trajectory_waypoint.waypoints[i].velocity[2] = trajectory.vel_z[i];
1755 trajectory_waypoint.waypoints[i].acceleration[0] = trajectory.acc_x[i];
1756 trajectory_waypoint.waypoints[i].acceleration[1] = trajectory.acc_y[i];
1757 trajectory_waypoint.waypoints[i].acceleration[2] = trajectory.acc_z[i];
1759 trajectory_waypoint.waypoints[i].yaw = trajectory.pos_yaw[i];
1760 trajectory_waypoint.waypoints[i].yaw_speed = trajectory.vel_yaw[i];
1762 trajectory_waypoint.waypoints[i].type = UINT8_MAX;
1765 for (
int i = 0; i < number_valid_points; ++i) {
1766 trajectory_waypoint.waypoints[i].point_valid =
true;
1769 for (
int i = number_valid_points; i < vehicle_trajectory_waypoint_s::NUMBER_POINTS; ++i) {
1770 trajectory_waypoint.waypoints[i].point_valid =
false;
1780 return (buttons & (1 << sw)) ? manual_control_setpoint_s::SWITCH_POS_ON : manual_control_setpoint_s::SWITCH_POS_OFF;
1786 bool on = (buttons & (1 << sw));
1793 unsigned state_count = (sw == 0) ? 3 : 2;
1796 if (!on && (on != last_on)) {
1814 return on * 1000 + 1000;
1821 mavlink_rc_channels_override_t man;
1822 mavlink_msg_rc_channels_override_decode(msg, &man);
1834 rc.timestamp_last_signal = rc.timestamp;
1836 rc.rc_failsafe =
false;
1838 rc.rc_lost_frame_count = 0;
1839 rc.rc_total_frame_count = 1;
1840 rc.rc_ppm_frame_length = 0;
1841 rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
1844 rc.values[0] = man.chan1_raw;
1845 rc.values[1] = man.chan2_raw;
1846 rc.values[2] = man.chan3_raw;
1847 rc.values[3] = man.chan4_raw;
1848 rc.values[4] = man.chan5_raw;
1849 rc.values[5] = man.chan6_raw;
1850 rc.values[6] = man.chan7_raw;
1851 rc.values[7] = man.chan8_raw;
1852 rc.values[8] = man.chan9_raw;
1853 rc.values[9] = man.chan10_raw;
1854 rc.values[10] = man.chan11_raw;
1855 rc.values[11] = man.chan12_raw;
1856 rc.values[12] = man.chan13_raw;
1857 rc.values[13] = man.chan14_raw;
1858 rc.values[14] = man.chan15_raw;
1859 rc.values[15] = man.chan16_raw;
1860 rc.values[16] = man.chan17_raw;
1861 rc.values[17] = man.chan18_raw;
1864 for (
int i = 17; i >= 0; i--) {
1865 const bool ignore_max = rc.values[i] == UINT16_MAX;
1866 const bool ignore_zero = (i > 7) && (rc.values[i] == 0);
1868 if (ignore_max || ignore_zero) {
1874 rc.channel_count = i + 1;
1886 mavlink_manual_control_t man;
1887 mavlink_msg_manual_control_decode(msg, &man);
1898 rc.timestamp_last_signal = rc.timestamp;
1900 rc.channel_count = 8;
1901 rc.rc_failsafe =
false;
1903 rc.rc_lost_frame_count = 0;
1904 rc.rc_total_frame_count = 1;
1905 rc.rc_ppm_frame_length = 0;
1906 rc.input_source = input_rc_s::RC_INPUT_SOURCE_MAVLINK;
1909 rc.values[0] = man.x / 2 + 1500;
1910 rc.values[1] = man.y / 2 + 1500;
1911 rc.values[2] = man.r / 2 + 1500;
1912 rc.values[3] =
math::constrain(man.z / 0.9f + 800.0f, 1000.0f, 2000.0f);
1915 unsigned max_switch = (
sizeof(man.buttons) * 8);
1916 unsigned max_channels = (
sizeof(rc.values) /
sizeof(rc.values[0]));
1918 if (max_switch > (max_channels - 4)) {
1919 max_switch = (max_channels - 4);
1923 for (
unsigned i = 0; i < max_switch; i++) {
1935 manual.x = man.x / 1000.0f;
1936 manual.y = man.y / 1000.0f;
1937 manual.r = man.r / 1000.0f;
1938 manual.z = man.z / 1000.0f;
1950 mavlink_heartbeat_t hb;
1951 mavlink_msg_heartbeat_decode(msg, &hb);
1955 && hb.type == MAV_TYPE_ONBOARD_CONTROLLER)) {
1975 if (msgId == MAVLINK_MSG_ID_HEARTBEAT) {
1979 if (data_rate > 0) {
1986 if (interval < -0.00001
f) {
1989 }
else if (interval > 0.00001
f) {
1990 rate = 1000000.0f / interval;
1996 bool found_id =
false;
2004 if (stream_name !=
nullptr) {
2010 return (found_id ? PX4_OK : PX4_ERROR);
2016 unsigned interval = 0;
2019 if (stream->get_id() == msgId) {
2020 interval = stream->get_interval();
2032 mavlink_hil_sensor_t imu;
2033 mavlink_msg_hil_sensor_decode(msg, &imu);
2041 float ias =
calc_IAS(imu.diff_pressure * 1e2f);
2046 airspeed.timestamp = timestamp;
2047 airspeed.indicated_airspeed_m_s = ias;
2048 airspeed.true_airspeed_m_s = tas;
2058 gyro.x_raw = imu.xgyro * 1000.0f;
2059 gyro.y_raw = imu.ygyro * 1000.0f;
2060 gyro.z_raw = imu.zgyro * 1000.0f;
2064 gyro.temperature = imu.temperature;
2080 accel.temperature = imu.temperature;
2090 mag.x_raw = imu.xmag * 1000.0f;
2091 mag.y_raw = imu.ymag * 1000.0f;
2092 mag.z_raw = imu.zmag * 1000.0f;
2105 baro.pressure = imu.abs_pressure;
2106 baro.temperature = imu.temperature;
2109 baro.device_id = 1234567;
2118 hil_battery_status.
timestamp = timestamp;
2119 hil_battery_status.voltage_v = 11.5f;
2120 hil_battery_status.voltage_filtered_v = 11.5f;
2121 hil_battery_status.current_a = 10.0f;
2122 hil_battery_status.discharged_mah = -1.0f;
2131 mavlink_hil_gps_t
gps;
2132 mavlink_msg_hil_gps_decode(msg, &gps);
2142 hil_gps.
lat = gps.lat;
2143 hil_gps.
lon = gps.lon;
2144 hil_gps.
alt = gps.alt;
2145 hil_gps.
eph = (float)gps.eph * 1e-2
f;
2146 hil_gps.
epv = (
float)gps.epv * 1e-2
f;
2150 hil_gps.
vel_m_s = (float)gps.vel * 1e-2f;
2169 mavlink_follow_target_t follow_target_msg;
2170 mavlink_msg_follow_target_decode(msg, &follow_target_msg);
2175 follow_target_topic.lat = follow_target_msg.lat * 1e-7;
2176 follow_target_topic.lon = follow_target_msg.lon * 1e-7;
2177 follow_target_topic.alt = follow_target_msg.alt;
2185 mavlink_landing_target_t landing_target;
2186 mavlink_msg_landing_target_decode(msg, &landing_target);
2188 if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) {
2192 landing_target_pose.abs_pos_valid =
true;
2193 landing_target_pose.x_abs = landing_target.x;
2194 landing_target_pose.y_abs = landing_target.y;
2195 landing_target_pose.z_abs = landing_target.z;
2204 mavlink_cellular_status_t
status;
2205 mavlink_msg_cellular_status_decode(msg, &status);
2210 cellular_status.status = status.status;
2211 cellular_status.type = status.type;
2212 cellular_status.quality = status.quality;
2213 cellular_status.mcc = status.mcc;
2214 cellular_status.mnc = status.mnc;
2215 cellular_status.lac = status.lac;
2216 cellular_status.cid = status.cid;
2224 mavlink_adsb_vehicle_t adsb;
2225 mavlink_msg_adsb_vehicle_decode(msg, &adsb);
2231 t.icao_address = adsb.ICAO_address;
2232 t.lat = adsb.lat * 1e-7;
2233 t.lon = adsb.lon * 1e-7;
2234 t.altitude_type = adsb.altitude_type;
2235 t.altitude = adsb.altitude / 1000.0f;
2236 t.heading = adsb.heading / 100.0f / 180.0f *
M_PI_F -
M_PI_F;
2237 t.hor_velocity = adsb.hor_velocity / 100.0f;
2238 t.ver_velocity = adsb.ver_velocity / 100.0f;
2239 memcpy(&t.callsign[0], &adsb.callsign[0],
sizeof(t.callsign));
2240 t.emitter_type = adsb.emitter_type;
2242 t.squawk = adsb.squawk;
2244 t.flags = transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE;
2246 if (adsb.flags & ADSB_FLAGS_VALID_COORDS) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS; }
2248 if (adsb.flags & ADSB_FLAGS_VALID_ALTITUDE) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE; }
2250 if (adsb.flags & ADSB_FLAGS_VALID_HEADING) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING; }
2252 if (adsb.flags & ADSB_FLAGS_VALID_VELOCITY) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY; }
2254 if (adsb.flags & ADSB_FLAGS_VALID_CALLSIGN) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN; }
2256 if (adsb.flags & ADSB_FLAGS_VALID_SQUAWK) { t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK; }
2266 mavlink_utm_global_position_t utm_pos;
2267 mavlink_msg_utm_global_position_decode(msg, &utm_pos);
2270 float vx = utm_pos.vx / 100.0f;
2271 float vy = utm_pos.vy / 100.0f;
2272 float vz = utm_pos.vz / 100.0f;
2277 t.lat = utm_pos.lat * 1e-7;
2278 t.lon = utm_pos.lon * 1e-7;
2279 t.altitude = utm_pos.alt / 1000.0f;
2280 t.altitude_type = ADSB_ALTITUDE_TYPE_GEOMETRIC;
2282 t.heading = atan2f(vy, vx);
2283 t.hor_velocity = sqrtf(vy * vy + vx * vx);
2284 t.ver_velocity = -vz;
2288 memset(&t.callsign[0], 0,
sizeof(t.callsign));
2289 t.emitter_type = ADSB_EMITTER_TYPE_NO_INFO;
2300 }
else if (time_passed > UINT8_MAX) {
2301 time_passed = UINT8_MAX;
2304 t.tslc = (uint8_t) time_passed;
2308 if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_POSITION_AVAILABLE) {
2309 t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS;
2312 if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_ALTITUDE_AVAILABLE) {
2313 t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
2316 if (utm_pos.flags & UTM_DATA_AVAIL_FLAGS_HORIZONTAL_VELO_AVAILABLE) {
2317 t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING;
2318 t.flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
2331 mavlink_collision_t collision;
2332 mavlink_msg_collision_decode(msg, &collision);
2337 collision_report.src = collision.src;
2338 collision_report.id = collision.id;
2339 collision_report.action = collision.action;
2340 collision_report.threat_level = collision.threat_level;
2341 collision_report.time_to_minimum_delta = collision.time_to_minimum_delta;
2342 collision_report.altitude_minimum_delta = collision.altitude_minimum_delta;
2343 collision_report.horizontal_minimum_delta = collision.horizontal_minimum_delta;
2351 mavlink_gps_rtcm_data_t gps_rtcm_data_msg;
2352 mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg);
2356 gps_inject_data_topic.
len =
math::min((
int)
sizeof(gps_rtcm_data_msg.data),
2357 (
int)
sizeof(uint8_t) * gps_rtcm_data_msg.len);
2358 gps_inject_data_topic.flags = gps_rtcm_data_msg.flags;
2359 memcpy(gps_inject_data_topic.data, gps_rtcm_data_msg.data,
2360 math::min((
int)
sizeof(gps_inject_data_topic.data), (
int)
sizeof(uint8_t) * gps_inject_data_topic.len));
2368 mavlink_hil_state_quaternion_t hil_state;
2369 mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
2378 airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2
f;
2379 airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2
f;
2391 q.
copyTo(hil_attitude.q);
2401 hil_global_pos.lat = hil_state.lat / ((double)1e7);
2402 hil_global_pos.lon = hil_state.lon / ((double)1e7);
2403 hil_global_pos.alt = hil_state.alt / 1000.0f;
2404 hil_global_pos.vel_n = hil_state.vx / 100.0f;
2405 hil_global_pos.vel_e = hil_state.vy / 100.0f;
2406 hil_global_pos.vel_d = hil_state.vz / 100.0f;
2407 hil_global_pos.eph = 2.0f;
2408 hil_global_pos.epv = 4.0f;
2415 double lat = hil_state.lat * 1e-7;
2416 double lon = hil_state.lon * 1e-7;
2436 hil_local_pos.xy_valid =
true;
2437 hil_local_pos.z_valid =
true;
2438 hil_local_pos.v_xy_valid =
true;
2439 hil_local_pos.v_z_valid =
true;
2440 hil_local_pos.x = x;
2441 hil_local_pos.y = y;
2443 hil_local_pos.vx = hil_state.vx / 100.0f;
2444 hil_local_pos.vy = hil_state.vy / 100.0f;
2445 hil_local_pos.vz = hil_state.vz / 100.0f;
2448 hil_local_pos.yaw = euler.psi();
2449 hil_local_pos.xy_global =
true;
2450 hil_local_pos.z_global =
true;
2451 hil_local_pos.vxy_max = INFINITY;
2452 hil_local_pos.vz_max = INFINITY;
2453 hil_local_pos.hagl_min = INFINITY;
2454 hil_local_pos.hagl_max = INFINITY;
2467 accel.x = hil_state.xacc;
2468 accel.y = hil_state.yacc;
2469 accel.z = hil_state.zacc;
2470 accel.temperature = 25.0f;
2480 gyro.x_raw = hil_state.rollspeed * 1e3f;
2481 gyro.y_raw = hil_state.pitchspeed * 1e3f;
2482 gyro.z_raw = hil_state.yawspeed * 1e3f;
2483 gyro.x = hil_state.rollspeed;
2484 gyro.y = hil_state.pitchspeed;
2485 gyro.z = hil_state.yawspeed;
2486 gyro.temperature = 25.0f;
2496 hil_battery_status.voltage_v = 11.1f;
2497 hil_battery_status.voltage_filtered_v = 11.1f;
2498 hil_battery_status.current_a = 10.0f;
2499 hil_battery_status.discharged_mah = -1.0f;
2508 mavlink_named_value_float_t debug_msg;
2509 mavlink_msg_named_value_float_decode(msg, &debug_msg);
2514 memcpy(debug_topic.key, debug_msg.name,
sizeof(debug_topic.key));
2515 debug_topic.key[
sizeof(debug_topic.key) - 1] =
'\0';
2516 debug_topic.value = debug_msg.value;
2524 mavlink_debug_t debug_msg;
2525 mavlink_msg_debug_decode(msg, &debug_msg);
2530 debug_topic.ind = debug_msg.ind;
2531 debug_topic.value = debug_msg.value;
2539 mavlink_debug_vect_t debug_msg;
2540 mavlink_msg_debug_vect_decode(msg, &debug_msg);
2545 memcpy(debug_topic.name, debug_msg.name,
sizeof(debug_topic.name));
2546 debug_topic.name[
sizeof(debug_topic.name) - 1] =
'\0';
2547 debug_topic.x = debug_msg.x;
2548 debug_topic.y = debug_msg.y;
2549 debug_topic.z = debug_msg.z;
2557 mavlink_debug_float_array_t debug_msg;
2558 mavlink_msg_debug_float_array_decode(msg, &debug_msg);
2563 debug_topic.id = debug_msg.array_id;
2564 memcpy(debug_topic.name, debug_msg.name,
sizeof(debug_topic.name));
2565 debug_topic.name[
sizeof(debug_topic.name) - 1] =
'\0';
2567 for (
size_t i = 0; i < debug_array_s::ARRAY_SIZE; i++) {
2568 debug_topic.data[i] = debug_msg.data[i];
2577 mavlink_onboard_computer_status_t status_msg;
2578 mavlink_msg_onboard_computer_status_decode(msg, &status_msg);
2583 onboard_computer_status_topic.uptime = status_msg.uptime;
2585 onboard_computer_status_topic.type = status_msg.type;
2587 memcpy(onboard_computer_status_topic.cpu_cores, status_msg.cpu_cores,
sizeof(status_msg.cpu_cores));
2588 memcpy(onboard_computer_status_topic.cpu_combined, status_msg.cpu_combined,
sizeof(status_msg.cpu_combined));
2589 memcpy(onboard_computer_status_topic.gpu_cores, status_msg.gpu_cores,
sizeof(status_msg.gpu_cores));
2590 memcpy(onboard_computer_status_topic.gpu_combined, status_msg.gpu_combined,
sizeof(status_msg.gpu_combined));
2591 onboard_computer_status_topic.temperature_board = status_msg.temperature_board;
2592 memcpy(onboard_computer_status_topic.temperature_core, status_msg.temperature_core,
2593 sizeof(status_msg.temperature_core));
2594 memcpy(onboard_computer_status_topic.fan_speed, status_msg.fan_speed,
sizeof(status_msg.fan_speed));
2595 onboard_computer_status_topic.ram_usage = status_msg.ram_usage;
2596 onboard_computer_status_topic.ram_total = status_msg.ram_total;
2597 memcpy(onboard_computer_status_topic.storage_type, status_msg.storage_type,
sizeof(status_msg.storage_type));
2598 memcpy(onboard_computer_status_topic.storage_usage, status_msg.storage_usage,
sizeof(status_msg.storage_usage));
2599 memcpy(onboard_computer_status_topic.storage_total, status_msg.storage_total,
sizeof(status_msg.storage_total));
2600 memcpy(onboard_computer_status_topic.link_type, status_msg.link_type,
sizeof(status_msg.link_type));
2601 memcpy(onboard_computer_status_topic.link_tx_rate, status_msg.link_tx_rate,
sizeof(status_msg.link_tx_rate));
2602 memcpy(onboard_computer_status_topic.link_rx_rate, status_msg.link_rx_rate,
sizeof(status_msg.link_rx_rate));
2603 memcpy(onboard_computer_status_topic.link_tx_max, status_msg.link_tx_max,
sizeof(status_msg.link_tx_max));
2604 memcpy(onboard_computer_status_topic.link_rx_max, status_msg.link_rx_max,
sizeof(status_msg.link_rx_max));
2614 mavlink_statustext_t statustext;
2615 mavlink_msg_statustext_decode(msg, &statustext);
2619 log_message.
severity = statustext.severity;
2622 snprintf(log_message.text,
sizeof(log_message.text),
2623 "[mavlink: component %d] %." STRINGIFY(MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN)
"s", msg->compid, statustext.text);
2637 char thread_name[17];
2639 px4_prctl(PR_SET_NAME, thread_name, px4_getpid());
2645 PX4_ERR(
"system boot did not complete in 20 seconds");
2653 const int timeout = 10;
2655 #if defined(__PX4_POSIX) 2657 uint8_t buf[1600 * 5];
2658 #elif defined(CONFIG_NET) 2665 mavlink_message_t
msg;
2667 struct pollfd fds[1] = {};
2671 fds[0].events = POLLIN;
2674 #if defined(MAVLINK_UDP) 2675 struct sockaddr_in srcaddr = {};
2676 socklen_t addrlen =
sizeof(srcaddr);
2680 fds[0].events = POLLIN;
2683 #endif // MAVLINK_UDP 2700 if (poll(&fds[0], 1, timeout) > 0) {
2707 const unsigned character_count = 20;
2710 if ((nread = ::
read(fds[0].
fd, buf,
sizeof(buf))) < (ssize_t)character_count) {
2712 px4_usleep(sleeptime);
2716 #if defined(MAVLINK_UDP) 2719 if (fds[0].revents & POLLIN) {
2720 nread = recvfrom(
_mavlink->
get_socket_fd(), buf,
sizeof(buf), 0, (
struct sockaddr *)&srcaddr, &addrlen);
2723 struct sockaddr_in &srcaddr_last =
_mavlink->get_client_source_address();
2725 int localhost = (127 << 24) + 1;
2727 if (!
_mavlink->get_client_source_initialized()) {
2735 || (srcaddr_last.sin_addr.s_addr == htonl(localhost))) {
2737 srcaddr_last.sin_addr.s_addr = srcaddr.sin_addr.s_addr;
2738 srcaddr_last.sin_port = srcaddr.sin_port;
2740 _mavlink->set_client_source_initialized();
2742 PX4_INFO(
"partner IP: %s", inet_ntoa(srcaddr.sin_addr));
2749 #endif // MAVLINK_UDP 2752 for (ssize_t i = 0; i < nread; i++) {
2792 #if defined(MAVLINK_UDP) 2795 #endif // MAVLINK_UDP 2800 if (t - last_send_update > timeout * 1000) {
2811 last_send_update = t;
2829 pthread_attr_t receiveloop_attr;
2830 pthread_attr_init(&receiveloop_attr);
2832 struct sched_param param;
2833 (void)pthread_attr_getschedparam(&receiveloop_attr, ¶m);
2834 param.sched_priority = SCHED_PRIORITY_MAX - 80;
2835 (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m);
2837 pthread_attr_setstacksize(&receiveloop_attr,
2842 pthread_attr_destroy(&receiveloop_attr);
void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust)
#define VEHICLE_TYPE_FIXED_WING
constexpr _Tp constrain(_Tp val, _Tp min_val, _Tp max_val)
void handle_message(const mavlink_message_t *msg)
void handle_message_vision_position_estimate(mavlink_message_t *msg)
int globallocalconverter_init(double lat_0, double lon_0, float alt_0, uint64_t timestamp)
Initialize the global mapping between global position (spherical) and local position (NED)...
MavlinkMissionManager _mission_manager
switch_pos_t decode_switch_pos(uint16_t buttons, unsigned sw)
Decode a switch position from a bitfield.
uORB::Subscription _vehicle_status_sub
void handle_message_set_mode(mavlink_message_t *msg)
uint8_t _mom_switch_pos[MOM_SWITCH_COUNT]
uORB::Publication< cellular_status_s > _cellular_status_pub
static struct vehicle_status_s status
static bool boot_complete()
uint16_t _mom_switch_state
void handle_message_statustext(mavlink_message_t *msg)
void check_active_mission(void)
bool get_forward_externalsp()
void handle_message_cellular_status(mavlink_message_t *msg)
void handle_message_manual_control(mavlink_message_t *msg)
uORB::Subscription _actuator_armed_sub
uORB::Publication< vehicle_global_position_s > _global_pos_pub
uORB::Publication< vehicle_gps_position_s > _gps_pub
Definition of geo / math functions to perform geodesic calculations.
uORB::Publication< airspeed_s > _airspeed_pub
float calc_TAS_from_EAS(float speed_equivalent, float pressure_ambient, float temperature_celsius)
Calculate true airspeed (TAS) from equivalent airspeed (EAS).
void send_flight_information()
map_projection_reference_s _hil_local_proj_ref
uORB::Publication< debug_key_value_s > _debug_key_value_pub
void handle_message_rc_channels_override(mavlink_message_t *msg)
bool evaluate_target_ok(int command, int target_system, int target_component)
void send(const hrt_abstime t)
Handle sending of messages.
uORB::Publication< debug_value_s > _debug_value_pub
void send(const hrt_abstime t)
Handle sending of messages.
void handle_message_command_ack(mavlink_message_t *msg)
__EXPORT void rotate_3f(enum Rotation rot, float &x, float &y, float &z)
rotate a 3 element float vector in-place
mavlink_status_t _status
receiver status, used for mavlink_parse_char()
void set_data_rate(int rate)
void acknowledge(uint8_t sysid, uint8_t compid, uint16_t command, uint8_t result)
void handle_ack(mavlink_logging_ack_t ack)
ack from mavlink for a data message
bool publish(const T &data)
Publish the struct.
void handle_message_named_value_float(mavlink_message_t *msg)
uORB::Publication< follow_target_s > _follow_target_pub
void handle_message_gps_global_origin(mavlink_message_t *msg)
A set of useful macros for enhanced runtime and compile time error detection and warning suppression...
uORB::Publication< vehicle_rates_setpoint_s > _rates_sp_pub
MAVLink 2.0 protocol interface definition.
void handle_message(mavlink_message_t *msg)
uORB::PublicationQueued< vehicle_command_s > _cmd_pub
void handle_message(const mavlink_message_t *msg)
void count_rxbytes(unsigned n)
Count received bytes.
uint8_t remote_component_id
void handle_message_set_attitude_target(mavlink_message_t *msg)
float calc_IAS(float differential_pressure)
Calculate indicated airspeed (IAS).
static hrt_abstime & get_first_start_time()
mavlink_status_t * get_status()
void handle_message_onboard_computer_status(mavlink_message_t *msg)
uint64_t get_start_time()
uORB::Publication< vehicle_odometry_s > _mocap_odometry_pub
uORB::PublicationMulti< sensor_baro_s > _baro_pub
uORB::PublicationQueued< gps_inject_data_s > _gps_inject_data_pub
uORB::Publication< vehicle_attitude_setpoint_s > _fw_virtual_att_sp_pub
void handle_message(const mavlink_message_t *msg)
void copyTo(Type dst[M *N]) const
void handle_message_set_actuator_control_target(mavlink_message_t *msg)
void handle_message_battery_status(mavlink_message_t *msg)
bool get_manual_input_mode_generation()
Get the manual input generation mode.
static constexpr float CONSTANTS_ONE_G
uORB::PublicationMulti< distance_sensor_s > _distance_sensor_pub
uORB::Publication< vehicle_odometry_s > _visual_odometry_pub
Type wrap_2pi(Type x)
Wrap value in range [0, 2Ï€)
void handle_message_debug(mavlink_message_t *msg)
bool _task_should_exit
Mavlink task should exit iff true.
Protocol get_protocol() const
uORB::PublicationMulti< input_rc_s > _rc_pub
static constexpr unsigned int MOM_SWITCH_COUNT
void get_message_interval(int msgId)
uORB::PublicationMulti< manual_control_setpoint_s > _manual_pub
static char msg[NUM_MSG][CONFIG_USART1_TXBUFSIZE]
static void read(bootloader_app_shared_t *pshared)
void Run()
Receive data from UART/UDP.
uORB::PublicationMulti< sensor_accel_s > _accel_pub
uORB::Publication< actuator_controls_s > _actuator_controls_pubs[4]
hrt_abstime _last_utm_global_pos_com
int32_t timestamp_time_relative
uORB::Publication< vehicle_attitude_setpoint_s > _mc_virtual_att_sp_pub
mavlink_system_t mavlink_system
void handle_message_hil_optical_flow(mavlink_message_t *msg)
static void * start_helper(void *context)
uORB::PublicationMulti< ping_s > _ping_pub
void handle_message_adsb_vehicle(mavlink_message_t *msg)
void handle_message_distance_sensor(mavlink_message_t *msg)
bool publish(const T &data)
Publish the struct.
void send_storage_information(int storage_id)
static int get_uart_fd(unsigned index)
#define MAVLINK_RECEIVER_NET_ADDED_STACK
void request_stop_ulog_streaming()
int decode_switch_pos_n(uint16_t buttons, unsigned sw)
Decode a switch position from a bitfield and state.
uORB::Publication< position_setpoint_triplet_s > _pos_sp_triplet_pub
void send_protocol_version()
Send the protocol version of MAVLink.
uORB::PublicationMulti< sensor_mag_s > _mag_pub
void handle_message(const mavlink_message_t *msg)
bool _hil_local_proj_inited
void handle_message_hil_gps(mavlink_message_t *msg)
void handle_message_command_int(mavlink_message_t *msg)
Rotation
Enum for board and external compass rotations.
uORB::PublicationMulti< sensor_gyro_s > _gyro_pub
MavlinkReceiver(Mavlink *parent)
constexpr T radians(T degrees)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
uint8_t remote_system_status
int get_instance_id() const
void handle_message_logging_ack(mavlink_message_t *msg)
uORB::Subscription _parameter_update_sub
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
uORB::PublicationQueued< vehicle_command_ack_s > _cmd_ack_pub
void handle_mavlink_command_ack(const mavlink_command_ack_t &ack, uint8_t from_sysid, uint8_t from_compid, uint8_t channel)
Handle mavlink command_ack.
void handle_message_radio_status(mavlink_message_t *msg)
uORB::Publication< vehicle_local_position_s > _local_pos_pub
uORB::Publication< debug_vect_s > _debug_vect_pub
uORB::Publication< vehicle_attitude_s > _attitude_pub
Mavlink commands sender with support for retransmission.
void handle_message_set_position_target_global_int(mavlink_message_t *msg)
#define TONE_ALARM0_DEVICE_PATH
void handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink, const vehicle_command_s &vehicle_command)
Common method to handle both mavlink command types.
uORB::PublicationQueued< transponder_report_s > _transponder_report_pub
mavlink_channel_t get_channel() const
struct ping_statistics_s & get_ping_statistics()
Get the ping statistics of this MAVLink link.
uORB::Publication< landing_target_pose_s > _landing_target_pose_pub
MavlinkShell * get_shell()
get the Mavlink shell.
void handle_message_hil_sensor(mavlink_message_t *msg)
bool updated()
Check if there is a new update.
uORB::Publication< log_message_s > _log_message_pub
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Driver for the PX4 audio alarm port, /dev/tone_alarm.
void handle_message_debug_vect(mavlink_message_t *msg)
uORB::Publication< vehicle_trajectory_waypoint_s > _trajectory_waypoint_pub
uint64_t sync_stamp(uint64_t usec)
Convert remote timestamp to local hrt time (usec) Use synchronised time if available, monotonic boot time otherwise.
constexpr _Tp min(_Tp a, _Tp b)
uORB::Publication< offboard_control_mode_s > _offboard_control_mode_pub
void handle_message(const mavlink_message_t *msg)
Handle possible FTP message.
uORB::Publication< optical_flow_s > _flow_pub
void handle_message_optical_flow_rad(mavlink_message_t *msg)
void handle_message_hil_state_quaternion(mavlink_message_t *msg)
void send_autopilot_capabilites()
Send the capabilities of this autopilot in terms of the MAVLink spec.
MavlinkULog * get_ulog_streaming()
get ulog streaming if active, nullptr otherwise
telemetry_status_s & get_telemetry_status()
Get the receive status of this MAVLink link.
void handle_message_play_tune(mavlink_message_t *msg)
int set_message_interval(int msgId, float interval, int data_rate=-1)
Set the interval at which the given message stream is published.
void send(const hrt_abstime t)
Handle sending of messages.
uORB::PublicationMulti< distance_sensor_s > _flow_distance_sensor_pub
void handle_message(const mavlink_message_t *msg)
const char * get_stream_name(const uint16_t msg_id)
void send(const hrt_abstime t)
Handle sending of messages.
MavlinkTimesync _mavlink_timesync
MavlinkParametersManager _parameters_manager
void handle_message_odometry(mavlink_message_t *msg)
int globallocalconverter_tolocal(double lat, double lon, float alt, float *x, float *y, float *z)
Convert from global position coordinates to local position coordinates using the global reference...
void handle_message_debug_float_array(mavlink_message_t *msg)
static float actuator_controls[output_max]
Quaternion< float > Quatf
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
uORB::Publication< collision_report_s > _collision_report_pub
uORB::Publication< vehicle_attitude_setpoint_s > _att_sp_pub
static void receive_start(pthread_t *thread, Mavlink *parent)
Start the receiver thread.
#define ORB_MULTI_MAX_INSTANCES
Maximum number of multi topic instances.
void handle_message_collision(mavlink_message_t *msg)
uORB::Publication< debug_array_s > _debug_array_pub
void handle_message_att_pos_mocap(mavlink_message_t *msg)
void handle_message_set_position_target_local_ned(mavlink_message_t *msg)
uORB::PublicationMulti< radio_status_s > _radio_status_pub
void try_start_ulog_streaming(uint8_t target_system, uint8_t target_component)
float calc_EAS_from_IAS(float speed_indicated, float scale)
Calculate equivalent airspeed (EAS) from indicated airspeed (IAS).
void configure_stream_threadsafe(const char *stream_name, float rate=-1.0f)
void handle_message_follow_target(mavlink_message_t *msg)
bool globallocalconverter_initialized()
Checks if globallocalconverter was initialized.
void send_statustext_critical(const char *string)
Send a status text with loglevel CRITICAL.
uORB::Publication< battery_status_s > _battery_pub
void set_proto_version(unsigned version)
Set the MAVLink version.
void close_shell()
close the Mavlink shell if it is open
size_t write(uint8_t *buffer, size_t len)
Write to the shell.
MavlinkLogHandler _mavlink_log_handler
uORB::Publication< obstacle_distance_s > _obstacle_distance_pub
void handle_message_heartbeat(mavlink_message_t *msg)
static MavlinkCommandSender & instance()
int get_system_id() const
Get the MAVLink system id.
List< MavlinkStream * > & get_streams()
unsigned get_system_type()
bool publish(const T &data)
Publish the struct.
void handle_message_landing_target(mavlink_message_t *msg)
void handle_message_utm_global_position(mavlink_message_t *msg)
int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0)
Initializes the map transformation given by the argument and sets the timestamp to now...
void handle_message_ping(mavlink_message_t *msg)
bool copy(void *dst)
Copy the struct.
uORB::Publication< onboard_computer_status_s > _onboard_computer_status_pub
uORB::Subscription _control_mode_sub
void handle_message_command_long(mavlink_message_t *msg)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
uORB::Subscription _vehicle_local_position_sub
void handle_message_gps_rtcm_data(mavlink_message_t *msg)
void handle_message_serial_control(mavlink_message_t *msg)
void handle_message_obstacle_distance(mavlink_message_t *msg)
void set_has_received_messages(bool received_messages)
static void set_boot_complete()
Set the boot complete flag on all instances.
uint64_t _global_ref_timestamp
void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg)
void update_radio_status(const radio_status_s &radio_status)